当我们使用拥有舵机的四轮车或其它拥有舵机的小车时,会发现串级pid的效果并不是特别的理想,这个时候我们可以使用模糊pid,先来简单介绍一下模糊pid的原理,模糊pid的核心在于动态P,根据输入的误差E和误差变化率EC来调整P。首先,我们要先在赛道上面测出小车的误差范围EFF与误差变化率的范围UFF(误差变化率的范围不好测或不会测的话可以设置为EFF的三分之一到二分之一)。例如,我们假设误差E的范围为【-15,15】,误差变化率的范围为【-6,+6】,将误差E七等份为【-15,-10,-5,0,5,10,15】,将误差变化率EC七等份为【-6,-4,-2,0,2,4,6】,每个位置分别对应着【负大,负中,负小,中,正小,正中,正大】。
我们以【E,EC】为【12,3】为例,E处于正中与正大中间,对于正中的隶属度为(12-10)/(15-10)=40%,对于正大的隶属度为(15-12)/(15-10)=60%。EC处于正小与正中中间,对于正小的隶属度为(3-2)/(4-2)=50%,对于正中的隶属度为(4-3)/(4-2)=50%。当【E,EC】=【正中,正小】时,根据上表(模糊规则表)对应的值为3,故3的隶属度为为40%×50%=20%,当【E,EC】=【正中,正中】时同理可得4的隶属度为20%,当【E,EC】=【正大,正小】时同理可得4的隶属度为30%,当【E,EC】=【正大,正中】时同理可得5的隶属度为30%,综上所述,输出为3×20%+4×20%+4×30%+5×30%=4.1,再将输出经过一个普通的位置式pid即可,这就是模糊pid大概的原理,为了方便大家理解,我用的都是通俗易的的俗语,没用专业术语,希望可以理解。
接下来为大家展示模糊pid代码的简单使用,首先是一些模糊pid的定义部分(这里用到的定义大家记得在.h文件里面声明一下):
#define PB 3 //正大
#define PM 2 //正中
#define PS 1 //正小
#define ZO 0 //中
#define NS -1 //负小
#define NM -2 //负中
#define NB -3 //负大
#define EC_FACTOR 1 //误差变化率的因子
#define ABS(x) (((x) > 0) ? (x) : (-(x)))
float EFF[7] ; //输入e的隶属值
float DFF[7] ; //输入de/dt的隶属值
int rule_p[7][7] = { {NB,NB,NM,NM,NS,ZO,ZO}, //kp规则表
{NB,NB,NM,NS,NS,ZO,NS},
{NM,NM,NM,NS,ZO,NS,NS},
{NM,NM,NS,ZO,NS,NM,NM},
{NS,NS,ZO,NS,NS,NM,NM},
{NS,ZO,NS,NM,NM,NM,NB},
{ZO,ZO,NM,NM,NM,NB,NB} };
int rule_d[7][7] = { {PS,NS,NB,NB,NB,NM,PS}, //kd规则表
{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} };
_Fuzzy_PD_t Turn_FuzzyPD = {
.Kp0 = 0.0,
.Kd0 = 0.0,
.threshold = 10,
.maximum = 15,
.minimum = -15,
.factor = 1.0,
.SetValue = 0.0,
};
然后是模糊pid参数的初始化,为了便于参数的调整,我们需要对模糊pid进行改进,我们需要设置p,d的最大值并对它进行线性分割(最大值的定义在最下方的模糊pid使用部分的代码,线性分割如下代码所示,将p,d线性分割后,最终输出时会在模糊pid输出的隶属度的基础上乘上各个p,d的隶属度,别急,往下看,后面的代码将会提到)
void fuzzy_init(float uff_p_max, float uff_d_max, _UFF_t* UFF,_Fuzzy_PD_t*Turn_FuzzyPD)
{
Turn_FuzzyPD->Kp0 = 50.0;//基础p(调参)
Turn_FuzzyPD->Kd0 = 30.0,//基础d(调参)
Turn_FuzzyPD->threshold = 10;//防止参数剧烈变化的临界值
Turn_FuzzyPD->maximum = 15;//输出最大限幅
Turn_FuzzyPD->minimum = -15;//输出最小限幅
Turn_FuzzyPD->factor = 1.0;//模糊系数
for(int i = 0; i < 7; i++)//初始化7个p和d的隶属度
{
UFF->UFF_P[i] = uff_p_max * ((float)(i-3) / 3);//将p化为7等份
UFF->UFF_D[i] = uff_d_max * ((float)(i-3) / 3);//将d化为7等份
}
}
接下来就是模糊pid的使用函数了(该函数的注释都写得非常详细,大家可自行观看,注意看看函数的输入和输出):
_DMF_t DMF;
float PID_FuzzyPD(float currentvalue, float* EFF, float* DFF, _Fuzzy_PD_t *Fuzzy_PD, _UFF_t* UFF,uint8 mode)//模糊pd
{
if(mode)
{
for(int i = 0; i < 7; i++)//初始化7个p和d的隶属度
{
EFF[i] = 21 * ((float)(i-3) / 3);//将误差范围21化为7等份(需自行测量替换)
DFF[i] = 18 * ((float)(i-3) / 3);//将误差变化率范围18化为7等份(需自行测量替换)
}
}
else
{
for(int i = 0; i < 7; i++)//初始化7个p和d的隶属度
{
EFF[i] = 18 * ((float)(i-3) / 3);//将误差范围18化为7等份(需自行测量替换)
DFF[i] = 9 * ((float)(i-3) / 3);//将误差变化率范围9化为7等份(需自行测量替换)
}
}
Fuzzy_PD->CurrentValue=currentvalue;//误差
Fuzzy_PD->err=Fuzzy_PD->SetValue-Fuzzy_PD->CurrentValue;//0-误差
float EC = Fuzzy_PD->err - Fuzzy_PD->errlast;//误差的变化率
count_DMF(Fuzzy_PD->err * Fuzzy_PD->factor, EC * Fuzzy_PD->factor * EC_FACTOR, EFF, DFF, &DMF);//模糊化
Fuzzy_PD->Kp = Fuzzy_PD->Kp0 + Fuzzy_Kp(&DMF, UFF);//反解模糊化并输出p(基础p+模糊p)
Fuzzy_PD->Kd = Fuzzy_PD->Kd0 + Fuzzy_Kd(&DMF, UFF);//反解模糊化并输出d(基础d+模糊d)
if (Fuzzy_PD->Kp < 0) Fuzzy_PD->Kp = 0;//保证p是正数
if (Fuzzy_PD->Kd < 0) Fuzzy_PD->Kd = 0;//保证d是正数
/*经过一个普通的位置式pd输出*/
Fuzzy_PD->out=Fuzzy_PD->Kp*Fuzzy_PD->err/100 + Fuzzy_PD->Kd*(Fuzzy_PD->err-Fuzzy_PD->errlast)/100;//(调参)
Fuzzy_PD->out=Fuzzy_PD->out*(-0.7);//(调参)
Fuzzy_PD->LeastValue=Fuzzy_PD->CurrentValue;
Fuzzy_PD->errlast=Fuzzy_PD->err;
Fuzzy_PD->errlastlast=Fuzzy_PD->errlast;
/*相当于一个滤波了,防止噪声*/
if (ABS(Fuzzy_PD->out - Fuzzy_PD->outlast) > Fuzzy_PD->threshold)
{
if(Fuzzy_PD->out > Fuzzy_PD->outlast)
Fuzzy_PD->out=Fuzzy_PD->outlast+Fuzzy_PD->threshold;
else
Fuzzy_PD->out=Fuzzy_PD->outlast-Fuzzy_PD->threshold;
}
/*输出限幅*/
if (Fuzzy_PD->out >= Fuzzy_PD->maximum)
Fuzzy_PD->out = Fuzzy_PD->maximum;
else if (Fuzzy_PD->out <= Fuzzy_PD->minimum)
Fuzzy_PD->out = Fuzzy_PD->minimum;
Fuzzy_PD->outlast=Fuzzy_PD->out;
return Fuzzy_PD->out;
}
上述模糊pid使用过程中用到的模糊化函数与反解模糊化函数给大家放在下面,(注意看经过规则表反解模糊后的各个隶属度又经过一个for循环乘上了各个p,d的隶属度才输出,大家看懂理解后可直接使用):
static void count_DMF(float e, float ec, float* EFF, float* DFF, _DMF_t* DMF)//模糊化e ec,用指针存到结构体DMF里,可直接使用
{
//求e的各个隶属度
if (e > EFF[0] && e < EFF[6])
{
for (int i = 0; i < 8 - 2; i++)
{
if (e >= EFF[i] && e <= EFF[i + 1])
{
DMF->EF[0] = -(e - EFF[i + 1]) / (EFF[i + 1] - EFF[i]);//隶属度
DMF->EF[1] = 1+(e - EFF[i + 1]) / (EFF[i + 1] - EFF[i]);//隶属度
DMF->En[0] = i;//隶属度在规则表对应的数值
DMF->En[1] = i + 1;//隶属度在规则表对应的数值
break;
}
}
}
else
{
if (e <= EFF[0])//超出范围
{
DMF->EF[0] = 1;
DMF->EF[1] = 0;
DMF->En[0] = 0;
DMF->En[1] = -1;
}
else if (e >= EFF[6])//超出范围
{
DMF->EF[0] = 1;
DMF->EF[1] = 0;
DMF->En[0] = 6;
DMF->En[1] = -1;
}
}
//求ec的各个隶属度
if (ec > DFF[0] && ec < DFF[6])
{
for (int i = 0; i < 8 - 2; i++)
{
if (ec >= DFF[i] && ec <= DFF[i + 1])
{
DMF->DF[0] = -(ec - DFF[i + 1]) / (DFF[i + 1] - DFF[i]);//隶属度
DMF->DF[1] = 1 + (ec - DFF[i + 1]) / (DFF[i + 1] - DFF[i]);//隶属度
DMF->Dn[0] = i;//隶属度在规则表对应的数值
DMF->Dn[1] = i + 1;//隶属度在规则表对应的数值
break;
}
}
}
else
{
if (ec <= DFF[0])//超出范围
{
DMF->DF[0] = 1;
DMF->DF[1] = 0;
DMF->Dn[0] = 0;
DMF->Dn[1] = -1;
}
else if (ec >= DFF[6])//超出范围
{
DMF->DF[0] = 1;
DMF->DF[1] = 0;
DMF->Dn[0] = 6;
DMF->Dn[1] = -1;
}
}
}
static float Fuzzy_Kp(_DMF_t* DMF, _UFF_t* UFF)//反解模糊p,可直接使用
{
float qdetail_kp;
float KpgradSums[7] = { 0,0,0,0,0,0,0 };
for (int i=0;i<2;i++)
{
if (DMF->En[i] == -1)//-1属于是超出规则表格了
{
continue;
}
for (int j = 0; j < 2; j++)
{
if (DMF->Dn[j] != -1)
{
int indexKp = rule_p[DMF->En[i]][DMF->Dn[j]] + 3;//U的隶属度,+3是为了下面的数组,规则表中是-3,对应下面数组的0
KpgradSums[indexKp]= KpgradSums[indexKp] + (DMF->EF[i] * DMF->DF[j]);//这个数组存的是u对于各个等级的隶属度的期望总和
}
else//同样ec规则索引等于-1说明这个索引不存在
{
continue;
}
}
}
for (int i = 0; i < 8 - 1; i++)
{
qdetail_kp += UFF->UFF_P[i] * KpgradSums[i];//输出各个p对应的隶属度总和
}
return qdetail_kp;//输出模糊p
}
static float Fuzzy_Kd(_DMF_t* DMF, _UFF_t* UFF)//反解模糊d,可直接使用
{
float qdetail_kd;
float KdgradSums[7] = { 0,0,0,0,0,0,0 };
for (int i=0;i<2;i++)
{
if (DMF->En[i] == -1)//-1属于是超出规则表格了
{
continue;
}
for (int j = 0; j < 2; j++)
{
if (DMF->Dn[j] != -1)
{
int indexKd = rule_d[DMF->En[i]][DMF->Dn[j]] + 3;
KdgradSums[indexKd] =KdgradSums[indexKd] + (DMF->EF[i] * DMF->DF[j]);//这个数组存的是u对于各个等级的隶属度的概率总和
}
else//同样ec规则索引等于-1说明这个索引不存在
{
continue;
}
}
}
for (int i = 0; i < 8 - 1; i++)
{
qdetail_kd+= UFF->UFF_D[i] * KdgradSums[i];//输出各个d对应的隶属度总和
}
return qdetail_kd;//输出模糊d
}
最后放上模糊pid的使用代码,这段代码放在主循环或者定时器中即可。
float Turn_UFF_kp_max = -60.0f; //模糊控制最大值p参数(调参)
float Turn_UFF_kd_max = 0.0f;//模糊控制最大值d参数(调参)
fuzzy_init(Turn_UFF_kp_max, Turn_UFF_kd_max, &Turn_UFF,&Turn_FuzzyPD);//模糊控制参数初始化
out_d = PID_FuzzyPD(-Point, EFF, DFF, &Turn_FuzzyPD, &Turn_UFF, 0);//模糊pid(Point为赛道中线偏差)
ServoMotorctrl(90+out_d);//舵机控制
模糊pid需要调整的参数我都已经在代码中注释,大家可根据注释自行调参。当然对于模糊pid的原理部分我讲解的可能不是特别细(也可以先去看看别人讲解原理的文章后再来观看我这篇使用教程),因为我主要还是想帮助大家快速用上模糊pid,且模糊pid也还有很多可以处理的更好的地方等待大家去处理。希望这篇文章对车友们能有所帮助,我的B站(同名)也会不定期更新车车视频,创作不易,希望大家能点点关注!!!一件三连!!!追求卓越,攀登高峰!!!