智能车模糊pid的简单使用教程

       当我们使用拥有舵机的四轮车或其它拥有舵机的小车时，会发现串级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站（同名）也会不定期更新车车视频，创作不易，希望大家能点点关注！！！一件三连！！！追求卓越，攀登高峰！！！