devbox
Cpp五条
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：Cpp五条 | 2024-06-08 19:44:32

赞

踩

智能车模糊控制

开源汇总写在下面

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

开源链接写在下面

https://gitee.com/joshua_xu/the-18th-smartcaricon-default.png?t=N7T8https://gitee.com/joshua_xu/the-18th-smartcar

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

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

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

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

一、电磁

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

这里介绍一下电磁控制。

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

电磁教程_某飞

这里介绍一下我的方案。

1.1控制方案

我的电感直立在舵机上

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

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

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

电磁板实物图

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

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

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

1.1.1 运放倍数

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

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

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

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

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

1.2 电磁数据处理

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

  1.     adc_init(ADC1_IN12_C2, ADC_12BIT);
  2.     adc_init(ADC1_IN13_C3, ADC_12BIT);
  3.     adc_init(ADC1_IN14_C4, ADC_12BIT);
  4.     adc_init(ADC1_IN15_C5, ADC_12BIT);

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

需要的基本变量如下：

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

数据处理需要分5步：

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

1.2.1 原始数据收集

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

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

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

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

1.2.2 取最大值，最小值

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

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

  1. 以第一个元素为基准。后面第二，第三，第四...个元素陆续和他进行比较。如果当前元素比他大，就让当前元素替换掉他（找最大值）。
  2. 以第一个元素为基准。后面第二，第三，第四...个元素陆续和他进行比较。如果当前元素比他小，就让当前元素替换掉他（找最小值）。
  1.     //将每个电感的第一次数据存下来
  2.     for(i=0;i<ADC_NUM;i++)//数据准备
  3.     {
  4.         ADC_Max_Value[i]=ADC_Raw_Value[i][0];
  5.         ADC_Min_Value[i]=ADC_Raw_Value[i][0];
  6.     }
  7.  
  8.     //后续数据和他比较
  9.     for(i=0;i<ADC_NUM;i++)//两重循环，取出每组adc的最大值，最小值
  10.     {
  11.         for(j=0;j<ADC_FILTER_TIME;j++)
  12.         {
  13.             if(ADC_Max_Value[i]<ADC_Raw_Value[i][j])
  14.                 ADC_Max_Value[i]=ADC_Raw_Value[i][j];
  15.             if(ADC_Min_Value[i]>ADC_Raw_Value[i][j])
  16.                 ADC_Min_Value[i]=ADC_Raw_Value[i][j];
  17.         }
  18.     }

1.2.3 取平均滤波

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

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

  1.   
  2.     for(i=0;i<ADC_NUM;i++)//5次数据求和
  3.     {
  4.         for(j=0;j<ADC_FILTER_TIME;j++)
  5.         {
  6.            ADC_Sum_Value[i]+=ADC_Raw_Value[i][j];
  7.         }
  8.     }
  9.  
  10. //（5次数据求和值-最大-最小）/3
  11. ADC_Fil_Value[i]=((float)(ADC_Sum_Value[i]-ADC_Max_Value[i]-ADC_Min_Value[i])/((float)ADC_FILTER_TIME-2));

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

大家直接使用即可。

  1.     int i=0,j=0;
  2.     for(i=0;i<ADC_NUM;i++)//数据准备
  3.     {
  4.         ADC_Max_Value[i]=ADC_Raw_Value[i][0];
  5.         ADC_Min_Value[i]=ADC_Raw_Value[i][0];
  6.         ADC_Sum_Value[i]=0;
  7.     }
  8.     for(i=0;i<ADC_NUM;i++)//两重循环，取出每组adc的最大值，最小值
  9.     {
  10.         for(j=0;j<ADC_FILTER_TIME;j++)
  11.         {
  12.             ADC_Sum_Value[i]+=ADC_Raw_Value[i][j];
  13.             if(ADC_Max_Value[i]<ADC_Raw_Value[i][j])
  14.                 ADC_Max_Value[i]=ADC_Raw_Value[i][j];
  15.             if(ADC_Min_Value[i]>ADC_Raw_Value[i][j])
  16.                 ADC_Min_Value[i]=ADC_Raw_Value[i][j];
  17.         }//然后求和，去掉最大最小，取平均
  18.         ADC_Fil_Value[i]=((float)(ADC_Sum_Value[i]-ADC_Max_Value[i]-ADC_Min_Value[i])/((float)ADC_FILTER_TIME-2));
  19.     }//此时的值就是滤波后的值

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之间，避免过多的小数浮点运算带来的误差以及性能的浪费。

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

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

1.2.5 差比和

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

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

电磁教程_某飞

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

1.2.6 出界

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

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

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

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

1.3 电磁元素处理

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

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

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

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

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

二、模糊控制

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  1. *-------------------------------------------------------------------------------------------------------------------
  2.   @brief     电磁PD控制
  3.   @param     err
  4.   @return    舵机打角值
  5.   Sample     Steer_Angle=PD(Err);//舵机PD调
  6.   @note      null
  7. -------------------------------------------------------------------------------------------------------------------*/
  8. int PD_ADC(float err)//舵机PD调节
  9. {
  10.    int  u;
  11.    float  P=1.18;//p动的
  12.    float  D=2.56;//d死的
  13.    volatile static float error_current,error_last;
  14.    float ek,ek1;
  15.    error_current=err;
  16.    ek=error_current;
  17.    ek1=error_current-error_last;
  18.    P=Fuzzy_P(err,ek1);//传统模糊PID
  19.    D=1.185;//d死的
  20.     u=P*ek+D*ek1;
  21.     error_last=error_current;
  22.     if(u>=LEFT_MAX)//限幅处理
  23.         u=LEFT_MAX;
  24.     else if(u<=RIGHT_MAX)//限幅处理
  25.         u=RIGHT_MAX;
  26.     return (int)u;
  27. }

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

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

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

2.2 模糊PID文件代码

Fuzzy.c文件内容如下：

  1. #include "zf_common_headfile.h"
  2. #include "Fuzzy.h"
  3.  
  4. #define PB  6
  5. #define PM  5
  6. #define PS  4
  7. #define ZO  3
  8. #define NS  2
  9. #define NM  1
  10. #define NB  0
  11.  
  12. float U=0;                            /*偏差,偏差微分以及输出值的精确量*/
  13. float PF[2]={0},DF[2]={0},UF[4]={0};  /*偏差,偏差微分以及输出值的隶属度*/
  14. int Pn=0,Dn=0,Un[4]={0};
  15. float t1=0,t2=0,t3=0,t4=0,temp1=0,temp2=0;
  16.  
  17. float Fuzzy_P(int E,int EC)
  18. {
  19.  
  20.  
  21. //只要改下面这几行参数
  22.     //这玩意没什么规律，p越大，转弯越好，直道会有抖动，p小转不过来，凭感觉调
  23.     //建议先用单套pd，看看车子正常的p大概在什么范围，下面的p就会有方向
  24. 	float EFF[7]={-100,-80,-60,0,60,80,100};//摄像头误差分区
  25.     /*输入量D语言值特征点*/
  26.     float DFF[7]={-80,-60,-20,0,20,60,80};//误差变化率分区
  27.     /*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
  28.     float UFF[7]={0,0.36,0.75,0.996,1.36953,1.7098,2.185};//限幅分区
  29. //只要改上面这几行参数
  30.  
  31.  
  32. 	int rule[7][7]={
  33.     //    0   1   2   3   4   5   6
  34.         { 6 , 5 , 4 , 3 , 2 , 1 , 0},//0
  35.         { 5 , 4 , 3 , 2 , 1 , 0 , 1},//1
  36.         { 4 , 3 , 2 , 1 , 0 , 1 , 2},//2
  37.         { 3 , 2 , 1 , 0 , 1 , 2 , 3},//3
  38.         { 2 , 1 , 0 , 1 , 2 , 3 , 4},//4
  39.         { 1 , 0 , 1 , 2 , 3 , 4 , 5},//5
  40.         { 0 , 1 , 2 , 3 , 4 , 5 , 6},//6
  41.     };
  42.  
  43.     /*隶属度的确定*/
  44.     /*根据PD的指定语言值获得有效隶属度*/
  45.     if((E>(*(EFF+0))) && (E<(*(EFF+6))))
  46.     {
  47.         if(E<=((*(EFF+1))))
  48.         {
  49.             Pn=-2;
  50.             *(PF+0)=((*(EFF+1))-E)/((*(EFF+1))-((*(EFF+0))));
  51.         }
  52.         else if(E<=((*(EFF+2))))
  53.         {
  54.             Pn=-1;
  55.             *(PF+0)=((*(EFF+2))-E)/((*(EFF+2))-(*(EFF+1)));
  56.         }
  57.         else if(E<=((*(EFF+3))))
  58.         {
  59.             Pn=0;
  60.             *(PF+0)=((*(EFF+3))-E)/((*(EFF+3))-(*(EFF+2)));
  61.         }
  62.         else if(E<=((*(EFF+4))))
  63.         {
  64.             Pn=1;
  65.             *(PF+0)=((*(EFF+4))-E)/((*(EFF+4))-(*(EFF+3)));
  66.         }
  67.         else if(E<=((*(EFF+5))))
  68.         {
  69.             Pn=2;
  70.             *(PF+0)=((*(EFF+5))-E)/((*(EFF+5))-(*(EFF+4)));
  71.         }
  72.         else if(E<=((*(EFF+6))))
  73.         {
  74.             Pn=3;
  75.             *(PF+0)=((*(EFF+6))-E)/((*(EFF+6))-(*(EFF+5)));
  76.         }
  77.     }
  78.  
  79.     else if(E<=((*(EFF+0))))
  80.     {
  81.         Pn=-2;
  82.         *(PF+0)=1;
  83.     }
  84.     else if(E>=((*(EFF+6))))
  85.     {
  86.         Pn=3;
  87.         *(PF+0)=0;
  88.     }
  89.  
  90.    *(PF+1)=1-(*(PF+0));
  91.  
  92.  
  93.     //判断D的隶属度
  94.     if(EC>(*(DFF+0))&&EC<(*(DFF+6)))
  95.     {
  96.         if(EC<=(*(DFF+1)))
  97.         {
  98.             Dn=-2;
  99.             (*(DF+0))=((*(DFF+1))-EC)/((*(DFF+1))-(*(DFF+0)));
  100.         }
  101.         else if(EC<=(*(DFF+2)))
  102.         {
  103.             Dn=-1;
  104.             (*(DF+0))=((*(DFF+2))-EC)/((*(DFF+2))-(*(DFF+1)));
  105.         }
  106.         else if(EC<=(*(DFF+3)))
  107.         {
  108.             Dn=0;
  109.             (*(DF+0))=((*(DFF+3))-EC)/((*(DFF+3))-(*(DFF+2)));
  110.         }
  111.         else if(EC<=(*(DFF+4)))
  112.         {
  113.             Dn=1;
  114.             (*(DF+0))=((*(DFF+4))-EC)/((*(DFF+4))-(*(DFF+3)));
  115.         }
  116.         else if(EC<=(*(DFF+5)))
  117.         {
  118.             Dn=2;
  119.             (*(DF+0))=((*(DFF+5))-EC)/((*(DFF+5))-(*(DFF+4)));
  120.         }
  121.         else if(EC<=(*(DFF+6)))
  122.         {
  123.             Dn=3;
  124.             (*(DF+0))=((*(DFF+6))-EC)/((*(DFF+6))-(*(DFF+5)));
  125.         }
  126.     }
  127.     //不在给定的区间内
  128.     else if (EC<=(*(DFF+0)))
  129.     {
  130.         Dn=-2;
  131.         (*(DF+0))=1;
  132.     }
  133.     else if(EC>=(*(DFF+6)))
  134.     {
  135.         Dn=3;
  136.         (*(DF+0))=0;
  137.     }
  138.  
  139.     DF[1]=1-(*(DF+0));
  140.  
  141.     /*使用误差范围优化后的规则表rule[7][7]*/
  142.     /*输出值使用13个隶属函数,中心值由UFF[7]指定*/
  143.     /*一般都是四个规则有效*/
  144.     Un[0]=rule[Pn+2][Dn+2];
  145.     Un[1]=rule[Pn+3][Dn+2];
  146.     Un[2]=rule[Pn+2][Dn+3];
  147.     Un[3]=rule[Pn+3][Dn+3];
  148.  
  149.     if((*(PF+0))<=(*(DF+0)))    //求小
  150.         (*(UF+0))=*(PF+0);
  151.     else
  152.         (*(UF+0))=(*(DF+0));
  153.     if((*(PF+1))<=(*(DF+0)))
  154.         (*(UF+1))=*(PF+1);
  155.     else
  156.         (*(UF+1))=(*(DF+0));
  157.     if((*(PF+0))<=DF[1])
  158.         (*(UF+2))=*(PF+0);
  159.     else
  160.         (*(UF+2))=DF[1];
  161.     if((*(PF+1))<=DF[1])
  162.         (*(UF+3))=*(PF+1);
  163.     else
  164.         (*(UF+3))=DF[1];
  165.     /*同隶属函数输出语言值求大*/
  166.     if(Un[0]==Un[1])
  167.     {
  168.         if(((*(UF+0)))>((*(UF+1))))
  169.             (*(UF+1))=0;
  170.         else
  171.             (*(UF+0))=0;
  172.     }
  173.     if(Un[0]==Un[2])
  174.     {
  175.         if(((*(UF+0)))>((*(UF+2))))
  176.             (*(UF+2))=0;
  177.         else
  178.             (*(UF+0))=0;
  179.     }
  180.     if(Un[0]==Un[3])
  181.     {
  182.         if((*(UF+0))>(*(UF+3)))
  183.             (*(UF+3))=0;
  184.         else
  185.             (*(UF+0))=0;
  186.     }
  187.     if(Un[1]==Un[2])
  188.     {
  189.         if((*(UF+1))>(*(UF+2)))
  190.             (*(UF+2))=0;
  191.         else
  192.             (*(UF+1))=0;
  193.     }
  194.     if(Un[1]==Un[3])
  195.     {
  196.         if((*(UF+1))>(*(UF+3)))
  197.             (*(UF+3))=0;
  198.         else
  199.             (*(UF+1))=0;
  200.     }
  201.     if(Un[2]==Un[3])
  202.     {
  203.         if((*(UF+2))>(*(UF+3)))
  204.             (*(UF+3))=0;
  205.         else
  206.             (*(UF+2))=0;
  207.     }
  208.     t1=((*(UF+0)))*(*(UFF+(*(Un+0))));
  209.     t2=((*(UF+1)))*(*(UFF+(*(Un+1))));
  210.     t3=((*(UF+2)))*(*(UFF+(*(Un+2))));
  211.     t4=((*(UF+3)))*(*(UFF+(*(Un+3))));
  212.     temp1=t1+t2+t3+t4;
  213.     temp2=(*(UF+0))+(*(UF+1))+(*(UF+2))+(*(UF+3));//模糊量输出
  214.     U=temp1/temp2;
  215.     return U;
  216. }
  217.  
  218. float Fuzzy_D(int  E,int EC)
  219. {
  220.  
  221. //只要改下面这几行参数
  222.     /*输入量P语言值特征点*/
  223.     float EFF[7]={-60,-40,12,0,12,25,60};//摄像头误差分区
  224.     /*输入量D语言值特征点*/
  225.     float DFF[7]={-40,-20,-7,0,7,20,40}; //误差变化率分区
  226.     /*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
  227.     float UFF[7]={0,1.5,1.9,2.6,3.9,4.3,5.836};      //限幅分区
  228. //只要改上面这几行参数
  229.  
  230.  
  231.     int rule[7][7]={
  232.     //    0  1    2  3    4   5   6
  233.         { 6 , 1 , 2 , 3 , 4 , 5 , 6},//0
  234.         { 1 , 2 , 3 , 4 , 5 , 6 , 5},//1
  235.         { 2 , 3 , 4 , 5 , 6 , 5 , 4},//2
  236.         { 3 , 4 , 5 , 6 , 5 , 4 , 3},//3
  237.         { 4 , 5 , 6 , 5 , 4 , 3 , 2},//4
  238.         { 5 , 6 , 5 , 4 , 3 , 2 , 1},//5
  239.         { 6 , 5 , 4 , 3 , 2 , 1 , 0},//6
  240.     };
  241.     int Pn=0,Dn=0,Un[4]={0};
  242.     /*隶属度的确定*/
  243.     /*根据PD的指定语言值获得有效隶属度*/
  244.     if((E>(*(EFF+0))) && (E<(*(EFF+6))))
  245.     {
  246.         if(E<=((*(EFF+1))))
  247.         {
  248.             Pn=-2;
  249.             *(PF+0)=((*(EFF+1))-E)/((*(EFF+1))-(*(EFF+0)));
  250.         }
  251.         else if(E<=((*(EFF+2))))
  252.         {
  253.             Pn=-1;
  254.             *(PF+0)=((*(EFF+2))-E)/((*(EFF+2))-(*(EFF+1)));
  255.         }
  256.         else if(E<=((*(EFF+3))))
  257.         {
  258.             Pn=0;
  259.             *(PF+0)=((*(EFF+3))-E)/((*(EFF+3))-(*(EFF+2)));
  260.         }
  261.         else if(E<=((*(EFF+4))))
  262.         {
  263.             Pn=1;
  264.             *(PF+0)=((*(EFF+4))-E)/((*(EFF+4))-(*(EFF+3)));
  265.         }
  266.         else if(E<=((*(EFF+5))))
  267.         {
  268.             Pn=2;
  269.             *(PF+0)=((*(EFF+5))-E)/((*(EFF+5))-(*(EFF+4)));
  270.         }
  271.         else if(E<=((*(EFF+6))))
  272.         {
  273.             Pn=3;
  274.             *(PF+0)=((*(EFF+6))-E)/((*(EFF+6))-(*(EFF+5)));
  275.         }
  276.     }
  277.  
  278.     else if(E<=((*(EFF+0))))
  279.     {
  280.         Pn=-2;
  281.         *(PF+0)=1;
  282.     }
  283.     else if(E>=((*(EFF+6))))
  284.     {
  285.         Pn=3;
  286.         *(PF+0)=0;
  287.     }
  288.  
  289.     *(PF+1)=1-*(PF+0);
  290.  
  291.  
  292.     //判断D的隶属度
  293.     if(EC>(*(DFF+0))&&EC<(*(DFF+6)))
  294.     {
  295.         if(EC<=(*(DFF+1)))
  296.         {
  297.             Dn=-2;
  298.             (*(DF+0))=((*(DFF+1))-EC)/((*(DFF+1))-(*(DFF+0)));
  299.         }
  300.         else if(EC<=(*(DFF+2)))
  301.         {
  302.             Dn=-1;
  303.             (*(DF+0))=((*(DFF+2))-EC)/((*(DFF+2))-(*(DFF+1)));
  304.         }
  305.         else if(EC<=(*(DFF+3)))
  306.         {
  307.             Dn=0;
  308.             (*(DF+0))=((*(DFF+3))-EC)/((*(DFF+3))-(*(DFF+2)));
  309.         }
  310.         else if(EC<=(*(DFF+4)))
  311.         {
  312.             Dn=1;
  313.             (*(DF+0))=((*(DFF+4))-EC)/((*(DFF+4))-(*(DFF+3)));
  314.         }
  315.         else if(EC<=(*(DFF+5)))
  316.         {
  317.             Dn=2;
  318.             (*(DF+0))=((*(DFF+5))-EC)/((*(DFF+5))-(*(DFF+4)));
  319.         }
  320.         else if(EC<=(*(DFF+6)))
  321.         {
  322.             Dn=3;
  323.             (*(DF+0))=((*(DFF+6))-EC)/((*(DFF+6))-(*(DFF+5)));
  324.         }
  325.     }
  326.     //不在给定的区间内
  327.     else if (EC<=(*(DFF+0)))
  328.     {
  329.         Dn=-2;
  330.         (*(DF+0))=1;
  331.     }
  332.     else if(EC>=(*(DFF+6)))
  333.     {
  334.         Dn=3;
  335.         (*(DF+0))=0;
  336.     }
  337.  
  338.     DF[1]=1-(*(DF+0));
  339.  
  340.     /*使用误差范围优化后的规则表rule[7][7]*/
  341.     /*输出值使用13个隶属函数,中心值由UFF[7]指定*/
  342.     /*一般都是四个规则有效*/
  343.     Un[0]=rule[Pn+2][Dn+2];
  344.     Un[1]=rule[Pn+3][Dn+2];
  345.     Un[2]=rule[Pn+2][Dn+3];
  346.     Un[3]=rule[Pn+3][Dn+3];
  347.  
  348.     if(*(PF+0)<=(*(DF+0)))    //求小
  349.         (*(UF+0))=*(PF+0);
  350.     else
  351.         (*(UF+0))=(*(DF+0));
  352.     if((*(PF+1))<=(*(DF+0)))
  353.         (*(UF+1))=*(PF+1);
  354.     else
  355.         (*(UF+1))=(*(DF+0));
  356.     if(*(PF+0)<=DF[1])
  357.         (*(UF+2))=*(PF+0);
  358.     else
  359.         (*(UF+2))=DF[1];
  360.     if((*(PF+1))<=DF[1])
  361.         (*(UF+3))=*(PF+1);
  362.     else
  363.         (*(UF+3))=DF[1];
  364.     /*同隶属函数输出语言值求大*/
  365.     if(Un[0]==Un[1])
  366.     {
  367.         if(((*(UF+0)))>((*(UF+1))))
  368.             (*(UF+1))=0;
  369.         else
  370.             (*(UF+0))=0;
  371.     }
  372.     if(Un[0]==Un[2])
  373.     {
  374.         if((*(UF+0))>(*(UF+2)))
  375.             (*(UF+2))=0;
  376.         else
  377.             (*(UF+0))=0;
  378.     }
  379.     if(Un[0]==Un[3])
  380.     {
  381.         if((*(UF+0))>(*(UF+3)))
  382.             (*(UF+3))=0;
  383.         else
  384.             (*(UF+0))=0;
  385.     }
  386.     if(Un[1]==Un[2])
  387.     {
  388.         if((*(UF+1))>(*(UF+2)))
  389.             (*(UF+2))=0;
  390.         else
  391.             (*(UF+1))=0;
  392.     }
  393.     if(Un[1]==Un[3])
  394.     {
  395.         if(((*(UF+1)))>(*(UF+3)))
  396.             (*(UF+3))=0;
  397.         else
  398.             (*(UF+1))=0;
  399.     }
  400.     if(Un[2]==Un[3])
  401.     {
  402.         if((*(UF+2))>(*(UF+3)))
  403.             (*(UF+3))=0;
  404.         else
  405.             (*(UF+2))=0;
  406.     }
  407.     t1=((*(UF+0)))*(*(UFF+(*(Un+0))));
  408.     t2=((*(UF+1)))*(*(UFF+(*(Un+1))));
  409.     t3=((*(UF+2)))*(*(UFF+(*(Un+2))));
  410.     t4=((*(UF+3)))*(*(UFF+(*(Un+3))));
  411.     temp1=t1+t2+t3+t4;
  412.     temp2=(*(UF+0))+(*(UF+1))+(*(UF+2))+(*(UF+3));//模糊量输出
  413.     U=temp1/temp2;
  414.     return U;
  415. }
  416.

Fuzzy.h文件内容如下：

  1. #ifndef __FUZZY_H__
  2. #define __FUZZY_H__
  3.  
  4. #include "zf_common_headfile.h"
  5.  
  6. float Fuzzy_P(int  E,int EC);//第一个参数是误差，第二个是误差变化率
  7. float Fuzzy_D(int  E,int EC);
  8.  
  9. #endif

2.2.3 模糊PID代码参数修改

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

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

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

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

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

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

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

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

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

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

希望能够帮助到一些人。

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Cpp五条/article/detail/690903
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号