第18届全国大学生智能汽车竞赛四轮车开源讲解【8】--电磁、模糊控制

本恩介绍了智能车电磁部分的基本控制方式和逻辑，通过代码分析了电磁数据的处理方式和步骤。也简单介绍了模糊PID的代码，以及使用方法。

Joshua.X

2182人浏览 · 2023-10-19 19:51:57

Joshua.X · 2023-10-19 19:51:57 发布

开源汇总写在下面

第18届全国大学生智能汽车竞赛四轮车开源讲解_Joshua.X的博客-CSDN博客

开源链接写在下面

https://gitee.com/joshua_xu/the-18th-smartcarhttps://gitee.com/joshua_xu/the-18th-smartcar

注：以下方案有可能会用到一个或者多个下述变量，以下变量均在开源【3】边线提取一章有讲。

const uint8  Standard_Road_Wide[MT9V03X_H];//标准赛宽数组
volatile int Left_Line[MT9V03X_H]; //左边线数组
volatile int Right_Line[MT9V03X_H];//右边线数组
volatile int Mid_Line[MT9V03X_H];  //中线数组
volatile int Road_Wide[MT9V03X_H]; //实际赛宽数组
volatile int White_Column[MT9V03X_W];//每列白列长度
volatile int Search_Stop_Line;     //搜索截止行,只记录长度，想要坐标需要用视野高度减去该值
volatile int Boundry_Start_Left;   //左右边界起始点
volatile int Boundry_Start_Right;  //第一个非丢线点,常规边界起始点
volatile int Left_Lost_Time;       //边界丢线数
volatile int Right_Lost_Time;
volatile int Both_Lost_Time;//两边同时丢线数
int Longest_White_Column_Left[2]; //最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1】是第某列
int Longest_White_Column_Right[2];//最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1】是第某列
int Left_Lost_Flag[MT9V03X_H] ; //左丢线数组，丢线置1，没丢线置0
int Right_Lost_Flag[MT9V03X_H]; //右丢线数组，丢线置1，没丢线置0

注：文章中所有参数，角点范围之类的东西仅作为参考，实际参数，请根据需要实际调整！！！！！！！！！

实际上，智能车所有参数都需要根据你的实际情况进行调整，万万不可照搬不误！！！！！

一、电磁

智能车的电磁和摄像头是目前室内两大寻迹方式，摄像头我们前文一直在介绍，后续还会继续介绍一些元素的判断。

这里介绍一下电磁控制。

先贴上某飞家的电磁教程。

电磁教程_某飞

这里介绍一下我的方案。

1.1控制方案

我的电磁直立在舵机上，几乎没有前瞻，因为电磁主要用于断路，断路区间不会很长，0前瞻跑慢一点足够了。

电磁我放了5颗电感，但是由于运放只有四路通道，其实只接通了四颗。分别是最左，最右

中间，还有偏左边的一颗。

左侧的排线接口为数据线，中间的排母是TOF测距模块。

在代码中我实际使用到的是最左，最右，和反面的一颗电感，总共三颗电感。

左右电感用来寻迹，中间电感用来检测环岛。

1.1.1 运放倍数

在跑车之前需要记得调整运放板的电位器（滑动变阻器）。需要保证车子正放时，两边对称的电感信号值是基本一致的。不然车子即使正放，由于数据不对称车，子仍然是歪着跑的。

这里还涉及到环岛的电感数值变大的问题，拧运放倍率时，记得要在普通赛道上调整。调整后，再去环岛看一下，是否数据到达软件设计的环岛判断阈值，以及在其他地方不能随意到达阈值。不然出库看到电感过大就判环岛，那就完了。

一定要记得跑车前看一下电磁情况。

1.2 电磁数据处理

电磁信号是通过运放芯片放大，通过单片机ADC读取，ADC精度可由初始化决定。

    adc_init(ADC1_IN12_C2, ADC_12BIT);
    adc_init(ADC1_IN13_C3, ADC_12BIT);
    adc_init(ADC1_IN14_C4, ADC_12BIT);
    adc_init(ADC1_IN15_C5, ADC_12BIT);

精度肯定尽可能的高，我这里最高开到了12Bit。（理论0-4095，实际到3500左右就封顶了，这和硬件设计有关）

需要的基本变量如下：

#define  ADC_FILTER_TIME   5
#define  ADC_NUM           4

volatile uint16 ADC_Raw_Value[ADC_NUM][ADC_FILTER_TIME];//adc原始数据,，第一维是指第某个电感，第二纬是五次的数据
volatile uint16 ADC_Max_Value[ADC_NUM];//adc每个通道最大值，用于滤波
volatile uint16 ADC_Min_Value[ADC_NUM];//adc每个通道最小值，用于滤波
volatile uint16 ADC_Sum_Value[ADC_NUM];//5个数据求和，去掉最大，最小值
volatile float ADC_Fil_Value[ADC_NUM];//adc滤波后的值
volatile float ADC_Nor_Value[ADC_NUM];//adc每个通道归一化后的值
volatile float ADC_Sum=0;
volatile float ADC_Dif=0;
volatile float ADC_Err=0;//最终使用的电磁误差

数据处理需要分5步：

原始数据收集
取最大值，最小值
取平均滤波
归一化处理
差比和

1.2.1 原始数据收集

首先是原始数据的收集。由于原始数据具有很高的偶然性。我在收集数据时收集5次数据，以便在后面对电磁数据进行滤波处理。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     电感数据获取
  @param     null
  @return    null
  Sample     ADC_Get_Value（）；
  @note      直接读数,放在二维数组里，第一维是第某个电感，第二纬是一次性读5个数据，五次数据
-------------------------------------------------------------------------------------------------------------------*/
void  ADC_Get_Value(void)//读值
{
    int i=0;
    for(i=0;i<ADC_FILTER_TIME;i++)
    {
        ADC_Raw_Value[0][i]=adc_convert(ADC1_IN12_C2);//最左
        ADC_Raw_Value[1][i]=adc_convert(ADC1_IN15_C5);//左2
        ADC_Raw_Value[2][i]=adc_convert(ADC1_IN13_C3);
        ADC_Raw_Value[3][i]=adc_convert(ADC1_IN14_C4);//最右
    }
}

我开了一层循环，每个电感收集5次数据，作为一轮数据。其中二维数据的第一维度是电感序号，第二纬度是电感第某次的数据值。

（其实开个二层循环更好，对于后续增加电感也很方便）

1.2.2 取最大值，最小值

在我们获取原始数据时，我们是获取了五次的数据值，这五次数据作为一轮。下面我们要找到这五次数据的最大，最小值。用于后续处理。

找最大最小值的方式很简单，

以第一个元素为基准。后面第二，第三，第四...个元素陆续和他进行比较。如果当前元素比他大，就让当前元素替换掉他（找最大值）。
以第一个元素为基准。后面第二，第三，第四...个元素陆续和他进行比较。如果当前元素比他小，就让当前元素替换掉他（找最小值）。

    //将每个电感的第一次数据存下来
    for(i=0;i<ADC_NUM;i++)//数据准备
    {
        ADC_Max_Value[i]=ADC_Raw_Value[i][0];
        ADC_Min_Value[i]=ADC_Raw_Value[i][0];
    }

    //后续数据和他比较
    for(i=0;i<ADC_NUM;i++)//两重循环，取出每组adc的最大值，最小值
    {
        for(j=0;j<ADC_FILTER_TIME;j++)
        {
            if(ADC_Max_Value[i]<ADC_Raw_Value[i][j])
                ADC_Max_Value[i]=ADC_Raw_Value[i][j];
            if(ADC_Min_Value[i]>ADC_Raw_Value[i][j])
                ADC_Min_Value[i]=ADC_Raw_Value[i][j];
        }
    }

1.2.3 取平均滤波

到这一步，我们已经获取到每一颗电感5次数据中的最大，最小值。

下面需要对这五次数据进行均值滤波。简单的说就是5次数据，去掉最大的，去掉最小的，剩下三个取平均。这样可以克服意外情况带来的数据读取偏差。

  
    for(i=0;i<ADC_NUM;i++)//5次数据求和
    {
        for(j=0;j<ADC_FILTER_TIME;j++)
        {
           ADC_Sum_Value[i]+=ADC_Raw_Value[i][j];
        }
    }

//（5次数据求和值-最大-最小）/3
ADC_Fil_Value[i]=((float)(ADC_Sum_Value[i]-ADC_Max_Value[i]-ADC_Min_Value[i])/((float)ADC_FILTER_TIME-2));

这里我是分开讲述的，我实际代码中，为了减小内存开销，是在写在一起的。

大家直接使用即可。

    int i=0,j=0;
    for(i=0;i<ADC_NUM;i++)//数据准备
    {
        ADC_Max_Value[i]=ADC_Raw_Value[i][0];
        ADC_Min_Value[i]=ADC_Raw_Value[i][0];
        ADC_Sum_Value[i]=0;
    }
    for(i=0;i<ADC_NUM;i++)//两重循环，取出每组adc的最大值，最小值
    {
        for(j=0;j<ADC_FILTER_TIME;j++)
        {
            ADC_Sum_Value[i]+=ADC_Raw_Value[i][j];
            if(ADC_Max_Value[i]<ADC_Raw_Value[i][j])
                ADC_Max_Value[i]=ADC_Raw_Value[i][j];
            if(ADC_Min_Value[i]>ADC_Raw_Value[i][j])
                ADC_Min_Value[i]=ADC_Raw_Value[i][j];
        }//然后求和，去掉最大最小，取平均
        ADC_Fil_Value[i]=((float)(ADC_Sum_Value[i]-ADC_Max_Value[i]-ADC_Min_Value[i])/((float)ADC_FILTER_TIME-2));
    }//此时的值就是滤波后的值

1.2.4 归一化处理

这里讲一下归一化处理，到这一步，我的电感数据理论上是从0~4095，（由于硬件平台差异，我的只能到3500）但是如果换一个信号发生器，这个数值会变的，具体变多少没人知道。

所以为了车模有更强的适应性，我们需要将数据进行归一化处理，所谓归一化，就是将数据从

0~4096映射到0~1。

如果我车模正放在自家赛道上，左右电感理论上数值差不多，那么左右值可能是都是1000，那么换了一个赛道，换一个信号发生器，电磁线直径都和自家实验室不一样，那么左右电感信号有可能是1500。车子稍微歪一点，带来的误差就会比原来要大，那么车子就会跑出问题。

为此，我们将所有数据进行归一化处理，看一下车子在当前赛道的最大值是多少，当前电感值占最大值的百分之多少。这样可以最大程度，增强车模稳定性。

归一化公式如下

$x=\frac{x-x_{min}}{x_{max}-x_{min}}$

其中的Xmin，在智能车里面就是0，因为电感最小值就是没有电感信号0.

电感最大值可以调整车模位置，进行寻找。

那么公式就化简成这样

$x=\frac{x}{x_{max}}$

这里的x就是滤波后的X的值。我在实际使用时，将归一化放大了100倍，让数据处于0~100之间，避免过多的小数浮点运算带来的误差以及性能的浪费。

 for(i=0;i<ADC_NUM;i++)//归一化处理
    {
        ADC_Nor_Value[i]=(ADC_Fil_Value[i]/3500)*100;//归一化后，放大100倍,数据处于0~100
    }//这里的3500是实测，我的这套硬件开的12bit，他的adc读满是3500

数据再经过归一化处理后，就变成了我们正常跑车时候使用的数据了。

1.2.5 差比和

我使用了最左，最右两个电感进行差比和运算，差比和的结果代表者车子距离中线（电磁线）的偏差。

具体差比和原理，大家看一下某飞的文章。

电磁教程_某飞

    ADC_Sum=ADC_Nor_Value[0]+ADC_Nor_Value[3];
    ADC_Dif=ADC_Nor_Value[0]-ADC_Nor_Value[3];
    if(ADC_Sum>10)
    {
        ADC_Err=(ADC_Dif/ADC_Sum)*100;//用于电磁控制的err从这里计算出来的
    }

1.2.6 出界

在之前的文章中，我有说过利用电磁来做出界保护，正好在电磁这里和大家说一下。

if(Stop_Flag==0)//在正常情况下
    {
        if(Barricade_Flag==0)//且不是在横断过程中
        {
            if(sum<5&&Img_Disappear_Flag==1)
            {//电感过小，图像丢失，还不是路障
                count++;
                if(count>=10)//有10次机会，当累计10次四个电感数据之和小于某一阈值，直接停车
                {
                    count=0;
                    Stop_Flag=2;//这个标志位给2是立刻停下，在control文件中有阐述
                }
            }
            else if(Search_Stop_Line>=60&&sum<=5)//或者最长白列很长，但是电感值很小
            {//总之，出界保护建立在电感值非常小，图像要么丢，要么很离谱，那么直接刹车保护
                Stop_Flag=2;
            }
        }
    }

我并不是电磁数据小就立刻停车，因为只要停车，就不会再启动了，除非重新发车。

我有个10次缓冲的机会。当累计10次电感值过小，才会触发保护。确保不轻易停车

1.3 电磁元素处理

我简单说一下电磁车的元素处理。

由于电磁车数据很少，基本只有电感这一套数来据源。

电感数据少，但是可以通过排布的不同，获取不同的数据，有横放，竖放，外八，内八，放在电感架顶部，电感架底部等多种做法不同的放置方法。

对不同赛道处理不同，比如外八/内八对弯道的预测效果比较好，横放对直道感应好一些。剩下的由于我对电磁涉猎不多，这里不展开讲解，大家有兴趣的自行查阅资料。

至于元素判断那就可以把所有电感归一化后的数据发送到上位机上，用曲线图展示出来，手推车看不同电感在经过不同元素的变化情况，找到元素的特征点，并进行对应打角，减速之类的处理。

二、模糊控制

个人使用的模糊控制是建立在pid的基础上，主要是对pid系数的控制。

2.1 本人对使用模糊控制的理解

我对模糊控制原理不是很懂，故不做讲解，主要讲使用。

想了解原理的，自行查阅资料，我就不带歪你们了。

首先，我对普通pd算法的理解。

p越大，转弯越迅速，但是对于直道越容易抖动。因为一点点的误差，乘以的p很大，那么反馈回去的值就很大，易发生震荡。

d越大，实测会引发高频震荡，因为d乘的（本次误差-上次误差），也就是误差变化率。他对未来的“预判”效果越严重，也会导致问题。

最简单的控制优化，就是分段pid，如果误差小于某一阈值，那么p就给小一点，大于某一阈值，p给大一点，这样可以在直道更容易稳定，弯道可以灵活转弯。

只分两段肯定是不够的，想要好一点可以多层分段，然而实际跑车过程中路况复杂，而且pd参数也不能只靠当前误差来确定。

那么我们就对误差进行超级细分。

以下是个人对模糊PID使用的理解，不见得对！仅供参考。

模糊控制这里我们引入了误差变化率，这一变量。

误差变化率=本次误差-上次误差；

在实际调用过程中如下：（我只用了模糊P，D是固定的）

*-------------------------------------------------------------------------------------------------------------------
  @brief     电磁PD控制
  @param     err
  @return    舵机打角值
  Sample     Steer_Angle=PD(Err);//舵机PD调
  @note      null
-------------------------------------------------------------------------------------------------------------------*/
int PD_ADC(float err)//舵机PD调节
{
   int  u;
   float  P=1.18;//p动的
   float  D=2.56;//d死的
   volatile static float error_current,error_last;
   float ek,ek1;
   error_current=err;
   ek=error_current;
   ek1=error_current-error_last;
   P=Fuzzy_P(err,ek1);//传统模糊PID
   D=1.185;//d死的
    u=P*ek+D*ek1;
    error_last=error_current;
    if(u>=LEFT_MAX)//限幅处理
        u=LEFT_MAX;
    else if(u<=RIGHT_MAX)//限幅处理
        u=RIGHT_MAX;
    return (int)u;
}

因为误差变化率pd里面有用到，我就直接调用了pd里面的ek1；

模糊PID就相当于一个二维函数，输入两个参数，输出一个结果。

输入的是误差，误差变化率，任意一个参数增大，会导致结果增大，两个参数增大，结果会更大。增大多少，需要考虑隶属度和模糊表，这个我们不必关心。

2.2 模糊PID文件代码

Fuzzy.c文件内容如下：

#include "zf_common_headfile.h"
#include "Fuzzy.h"

#define PB  6
#define PM  5
#define PS  4
#define ZO  3
#define NS  2
#define NM  1
#define NB  0

float U=0;                            /*偏差,偏差微分以及输出值的精确量*/
float PF[2]={0},DF[2]={0},UF[4]={0};  /*偏差,偏差微分以及输出值的隶属度*/
int Pn=0,Dn=0,Un[4]={0};
float t1=0,t2=0,t3=0,t4=0,temp1=0,temp2=0;

float Fuzzy_P(int E,int EC)
{


//只要改下面这几行参数
    //这玩意没什么规律，p越大，转弯越好，直道会有抖动，p小转不过来，凭感觉调
    //建议先用单套pd，看看车子正常的p大概在什么范围，下面的p就会有方向
	float EFF[7]={-100,-80,-60,0,60,80,100};//摄像头误差分区
    /*输入量D语言值特征点*/
    float DFF[7]={-80,-60,-20,0,20,60,80};//误差变化率分区
    /*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
    float UFF[7]={0,0.36,0.75,0.996,1.36953,1.7098,2.185};//限幅分区
//只要改上面这几行参数


	int rule[7][7]={
    //    0   1   2   3   4   5   6
        { 6 , 5 , 4 , 3 , 2 , 1 , 0},//0
        { 5 , 4 , 3 , 2 , 1 , 0 , 1},//1
        { 4 , 3 , 2 , 1 , 0 , 1 , 2},//2
        { 3 , 2 , 1 , 0 , 1 , 2 , 3},//3
        { 2 , 1 , 0 , 1 , 2 , 3 , 4},//4
        { 1 , 0 , 1 , 2 , 3 , 4 , 5},//5
        { 0 , 1 , 2 , 3 , 4 , 5 , 6},//6
    };

    /*隶属度的确定*/
    /*根据PD的指定语言值获得有效隶属度*/
    if((E>(*(EFF+0))) && (E<(*(EFF+6))))
    {
        if(E<=((*(EFF+1))))
        {
            Pn=-2;
            *(PF+0)=((*(EFF+1))-E)/((*(EFF+1))-((*(EFF+0))));
        }
        else if(E<=((*(EFF+2))))
        {
            Pn=-1;
            *(PF+0)=((*(EFF+2))-E)/((*(EFF+2))-(*(EFF+1)));
        }
        else if(E<=((*(EFF+3))))
        {
            Pn=0;
            *(PF+0)=((*(EFF+3))-E)/((*(EFF+3))-(*(EFF+2)));
        }
        else if(E<=((*(EFF+4))))
        {
            Pn=1;
            *(PF+0)=((*(EFF+4))-E)/((*(EFF+4))-(*(EFF+3)));
        }
        else if(E<=((*(EFF+5))))
        {
            Pn=2;
            *(PF+0)=((*(EFF+5))-E)/((*(EFF+5))-(*(EFF+4)));
        }
        else if(E<=((*(EFF+6))))
        {
            Pn=3;
            *(PF+0)=((*(EFF+6))-E)/((*(EFF+6))-(*(EFF+5)));
        }
    }

    else if(E<=((*(EFF+0))))
    {
        Pn=-2;
        *(PF+0)=1;
    }
    else if(E>=((*(EFF+6))))
    {
        Pn=3;
        *(PF+0)=0;
    }

   *(PF+1)=1-(*(PF+0));


    //判断D的隶属度
    if(EC>(*(DFF+0))&&EC<(*(DFF+6)))
    {
        if(EC<=(*(DFF+1)))
        {
            Dn=-2;
            (*(DF+0))=((*(DFF+1))-EC)/((*(DFF+1))-(*(DFF+0)));
        }
        else if(EC<=(*(DFF+2)))
        {
            Dn=-1;
            (*(DF+0))=((*(DFF+2))-EC)/((*(DFF+2))-(*(DFF+1)));
        }
        else if(EC<=(*(DFF+3)))
        {
            Dn=0;
            (*(DF+0))=((*(DFF+3))-EC)/((*(DFF+3))-(*(DFF+2)));
        }
        else if(EC<=(*(DFF+4)))
        {
            Dn=1;
            (*(DF+0))=((*(DFF+4))-EC)/((*(DFF+4))-(*(DFF+3)));
        }
        else if(EC<=(*(DFF+5)))
        {
            Dn=2;
            (*(DF+0))=((*(DFF+5))-EC)/((*(DFF+5))-(*(DFF+4)));
        }
        else if(EC<=(*(DFF+6)))
        {
            Dn=3;
            (*(DF+0))=((*(DFF+6))-EC)/((*(DFF+6))-(*(DFF+5)));
        }
    }
    //不在给定的区间内
    else if (EC<=(*(DFF+0)))
    {
        Dn=-2;
        (*(DF+0))=1;
    }
    else if(EC>=(*(DFF+6)))
    {
        Dn=3;
        (*(DF+0))=0;
    }

    DF[1]=1-(*(DF+0));

    /*使用误差范围优化后的规则表rule[7][7]*/
    /*输出值使用13个隶属函数,中心值由UFF[7]指定*/
    /*一般都是四个规则有效*/
    Un[0]=rule[Pn+2][Dn+2];
    Un[1]=rule[Pn+3][Dn+2];
    Un[2]=rule[Pn+2][Dn+3];
    Un[3]=rule[Pn+3][Dn+3];

    if((*(PF+0))<=(*(DF+0)))    //求小
        (*(UF+0))=*(PF+0);
    else
        (*(UF+0))=(*(DF+0));
    if((*(PF+1))<=(*(DF+0)))
        (*(UF+1))=*(PF+1);
    else
        (*(UF+1))=(*(DF+0));
    if((*(PF+0))<=DF[1])
        (*(UF+2))=*(PF+0);
    else
        (*(UF+2))=DF[1];
    if((*(PF+1))<=DF[1])
        (*(UF+3))=*(PF+1);
    else
        (*(UF+3))=DF[1];
    /*同隶属函数输出语言值求大*/
    if(Un[0]==Un[1])
    {
        if(((*(UF+0)))>((*(UF+1))))
            (*(UF+1))=0;
        else
            (*(UF+0))=0;
    }
    if(Un[0]==Un[2])
    {
        if(((*(UF+0)))>((*(UF+2))))
            (*(UF+2))=0;
        else
            (*(UF+0))=0;
    }
    if(Un[0]==Un[3])
    {
        if((*(UF+0))>(*(UF+3)))
            (*(UF+3))=0;
        else
            (*(UF+0))=0;
    }
    if(Un[1]==Un[2])
    {
        if((*(UF+1))>(*(UF+2)))
            (*(UF+2))=0;
        else
            (*(UF+1))=0;
    }
    if(Un[1]==Un[3])
    {
        if((*(UF+1))>(*(UF+3)))
            (*(UF+3))=0;
        else
            (*(UF+1))=0;
    }
    if(Un[2]==Un[3])
    {
        if((*(UF+2))>(*(UF+3)))
            (*(UF+3))=0;
        else
            (*(UF+2))=0;
    }
    t1=((*(UF+0)))*(*(UFF+(*(Un+0))));
    t2=((*(UF+1)))*(*(UFF+(*(Un+1))));
    t3=((*(UF+2)))*(*(UFF+(*(Un+2))));
    t4=((*(UF+3)))*(*(UFF+(*(Un+3))));
    temp1=t1+t2+t3+t4;
    temp2=(*(UF+0))+(*(UF+1))+(*(UF+2))+(*(UF+3));//模糊量输出
    U=temp1/temp2;
    return U;
}

float Fuzzy_D(int  E,int EC)
{

//只要改下面这几行参数
    /*输入量P语言值特征点*/
    float EFF[7]={-60,-40,12,0,12,25,60};//摄像头误差分区
    /*输入量D语言值特征点*/
    float DFF[7]={-40,-20,-7,0,7,20,40}; //误差变化率分区
    /*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
    float UFF[7]={0,1.5,1.9,2.6,3.9,4.3,5.836};      //限幅分区
//只要改上面这几行参数


    int rule[7][7]={
    //    0  1    2  3    4   5   6
        { 6 , 1 , 2 , 3 , 4 , 5 , 6},//0
        { 1 , 2 , 3 , 4 , 5 , 6 , 5},//1
        { 2 , 3 , 4 , 5 , 6 , 5 , 4},//2
        { 3 , 4 , 5 , 6 , 5 , 4 , 3},//3
        { 4 , 5 , 6 , 5 , 4 , 3 , 2},//4
        { 5 , 6 , 5 , 4 , 3 , 2 , 1},//5
        { 6 , 5 , 4 , 3 , 2 , 1 , 0},//6
    };
    int Pn=0,Dn=0,Un[4]={0};
    /*隶属度的确定*/
    /*根据PD的指定语言值获得有效隶属度*/
    if((E>(*(EFF+0))) && (E<(*(EFF+6))))
    {
        if(E<=((*(EFF+1))))
        {
            Pn=-2;
            *(PF+0)=((*(EFF+1))-E)/((*(EFF+1))-(*(EFF+0)));
        }
        else if(E<=((*(EFF+2))))
        {
            Pn=-1;
            *(PF+0)=((*(EFF+2))-E)/((*(EFF+2))-(*(EFF+1)));
        }
        else if(E<=((*(EFF+3))))
        {
            Pn=0;
            *(PF+0)=((*(EFF+3))-E)/((*(EFF+3))-(*(EFF+2)));
        }
        else if(E<=((*(EFF+4))))
        {
            Pn=1;
            *(PF+0)=((*(EFF+4))-E)/((*(EFF+4))-(*(EFF+3)));
        }
        else if(E<=((*(EFF+5))))
        {
            Pn=2;
            *(PF+0)=((*(EFF+5))-E)/((*(EFF+5))-(*(EFF+4)));
        }
        else if(E<=((*(EFF+6))))
        {
            Pn=3;
            *(PF+0)=((*(EFF+6))-E)/((*(EFF+6))-(*(EFF+5)));
        }
    }

    else if(E<=((*(EFF+0))))
    {
        Pn=-2;
        *(PF+0)=1;
    }
    else if(E>=((*(EFF+6))))
    {
        Pn=3;
        *(PF+0)=0;
    }

    *(PF+1)=1-*(PF+0);


    //判断D的隶属度
    if(EC>(*(DFF+0))&&EC<(*(DFF+6)))
    {
        if(EC<=(*(DFF+1)))
        {
            Dn=-2;
            (*(DF+0))=((*(DFF+1))-EC)/((*(DFF+1))-(*(DFF+0)));
        }
        else if(EC<=(*(DFF+2)))
        {
            Dn=-1;
            (*(DF+0))=((*(DFF+2))-EC)/((*(DFF+2))-(*(DFF+1)));
        }
        else if(EC<=(*(DFF+3)))
        {
            Dn=0;
            (*(DF+0))=((*(DFF+3))-EC)/((*(DFF+3))-(*(DFF+2)));
        }
        else if(EC<=(*(DFF+4)))
        {
            Dn=1;
            (*(DF+0))=((*(DFF+4))-EC)/((*(DFF+4))-(*(DFF+3)));
        }
        else if(EC<=(*(DFF+5)))
        {
            Dn=2;
            (*(DF+0))=((*(DFF+5))-EC)/((*(DFF+5))-(*(DFF+4)));
        }
        else if(EC<=(*(DFF+6)))
        {
            Dn=3;
            (*(DF+0))=((*(DFF+6))-EC)/((*(DFF+6))-(*(DFF+5)));
        }
    }
    //不在给定的区间内
    else if (EC<=(*(DFF+0)))
    {
        Dn=-2;
        (*(DF+0))=1;
    }
    else if(EC>=(*(DFF+6)))
    {
        Dn=3;
        (*(DF+0))=0;
    }

    DF[1]=1-(*(DF+0));

    /*使用误差范围优化后的规则表rule[7][7]*/
    /*输出值使用13个隶属函数,中心值由UFF[7]指定*/
    /*一般都是四个规则有效*/
    Un[0]=rule[Pn+2][Dn+2];
    Un[1]=rule[Pn+3][Dn+2];
    Un[2]=rule[Pn+2][Dn+3];
    Un[3]=rule[Pn+3][Dn+3];

    if(*(PF+0)<=(*(DF+0)))    //求小
        (*(UF+0))=*(PF+0);
    else
        (*(UF+0))=(*(DF+0));
    if((*(PF+1))<=(*(DF+0)))
        (*(UF+1))=*(PF+1);
    else
        (*(UF+1))=(*(DF+0));
    if(*(PF+0)<=DF[1])
        (*(UF+2))=*(PF+0);
    else
        (*(UF+2))=DF[1];
    if((*(PF+1))<=DF[1])
        (*(UF+3))=*(PF+1);
    else
        (*(UF+3))=DF[1];
    /*同隶属函数输出语言值求大*/
    if(Un[0]==Un[1])
    {
        if(((*(UF+0)))>((*(UF+1))))
            (*(UF+1))=0;
        else
            (*(UF+0))=0;
    }
    if(Un[0]==Un[2])
    {
        if((*(UF+0))>(*(UF+2)))
            (*(UF+2))=0;
        else
            (*(UF+0))=0;
    }
    if(Un[0]==Un[3])
    {
        if((*(UF+0))>(*(UF+3)))
            (*(UF+3))=0;
        else
            (*(UF+0))=0;
    }
    if(Un[1]==Un[2])
    {
        if((*(UF+1))>(*(UF+2)))
            (*(UF+2))=0;
        else
            (*(UF+1))=0;
    }
    if(Un[1]==Un[3])
    {
        if(((*(UF+1)))>(*(UF+3)))
            (*(UF+3))=0;
        else
            (*(UF+1))=0;
    }
    if(Un[2]==Un[3])
    {
        if((*(UF+2))>(*(UF+3)))
            (*(UF+3))=0;
        else
            (*(UF+2))=0;
    }
    t1=((*(UF+0)))*(*(UFF+(*(Un+0))));
    t2=((*(UF+1)))*(*(UFF+(*(Un+1))));
    t3=((*(UF+2)))*(*(UFF+(*(Un+2))));
    t4=((*(UF+3)))*(*(UFF+(*(Un+3))));
    temp1=t1+t2+t3+t4;
    temp2=(*(UF+0))+(*(UF+1))+(*(UF+2))+(*(UF+3));//模糊量输出
    U=temp1/temp2;
    return U;
}

Fuzzy.h文件内容如下：

#ifndef __FUZZY_H__
#define __FUZZY_H__

#include "zf_common_headfile.h"

float Fuzzy_P(int  E,int EC);//第一个参数是误差，第二个是误差变化率
float Fuzzy_D(int  E,int EC);

#endif

2.2.3 模糊PID代码参数修改

使用模糊PID代码时候，只要修改下面这三个地方就好。

//只要改下面这几行参数
    //这玩意没什么规律，p越大，转弯越好，直道会有抖动，p小转不过来，凭感觉调
    //建议先用单套pd，看看车子正常的p大概在什么范围，下面的p就会有方向
	float EFF[7]={-100,-80,-60,0,60,80,100};//摄像头误差分区
    /*输入量D语言值特征点*/
    float DFF[7]={-80,-60,-20,0,20,60,80};//误差变化率分区
    /*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
    float UFF[7]={0,0.36,0.75,0.996,1.36953,1.7098,2.185};//限幅分区
//只要改上面这几行参数

误差
误差变化率
限幅

这三个数组我是这么理解的。

误差：中间是0，最左最右是误差最大最小值。

中间就根据误差实际情况，找到经常出现的误差值

误差变化率：中间是0，最左最右是变化率的最大最小值

中间值就根据实际情况，选取经常出现的值。

每次进行模糊运算时，根据你的误差，误差变化率在哪一个限幅区间，在限幅区间中，按照一定的规则选择这个区间中的某一个值，作为本次的模糊结果，

限幅的话就基于常规pd系数，常规状态下的pd系数稍微放大一点，给到最值，中间就合划分即可。

整体来说，模糊PID调节是非常模糊的，就是一个凭着感觉调，没啥好经验。

希望能够帮助到一些人。

本人菜鸡一只，各位大佬发现问题欢迎留言指出。

华为开发者联盟HarmonyOS专区

鸿蒙生态一站式服务平台。

更多推荐

【grafana】使用教程

华为开发者联盟HarmonyOS专区

【PX4-AutoPilot教程-开发环境】使用VMware虚拟机安装Ubuntu系统并搭建PX4开发环境（ROS+mavros+jMAVSim+gazebo+QGC+QT）

学习PX4开发需要先配置好开发环境，对于新手推荐使用VMware虚拟机搭建Ubuntu系统，并下载PX4源码，配置好编译环境和工具链（ROS操作系统+mavros通信包+jMAVSim仿真+gazebo仿真+QGC地面站+QT开发平台）。教程中使用的是Ubuntu18.04系统（官方推荐使用版本），PX4固件版本为v1.13.0，飞控板为pixhawk2.4.8版本。