智能车模糊pid的简单使用教程
星辰pid 2024-08-16 15:01:02 阅读 72
当我们使用拥有舵机的四轮车或其它拥有舵机的小车时，会发现串级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文件里面声明一下）：
<code>#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站（同名）也会不定期更新车车视频，创作不易，希望大家能点点关注！！！一件三连！！！追求卓越，攀登高峰！！！
声明
本文内容仅代表作者观点，或转载于其他网站，本站不以此文作为商业用途
如有涉及侵权，请联系本站进行删除
转载本站原创文章，请注明来源及作者。