基本原理

image.png

模糊规则表

image.png

代码实现

  • 模块化设计
  • 需要配合C620C610电机群模块实现具体功能

    Math_FuzzyPID.h

    ```c /*
    • Copyright (c) 2020 - ~, HIT_HERO Team *
    • FUZZYPID MODULE HEAD FILE
    • Used in RT-Thread Operate System *
    • Change Logs:
    • Date Author Notes Mail
    • 2020-08-20 WangXi first version WangXi_chn@foxmail.com
    • Note: */

      ifndef MATH_FUZZYPID_H

      define MATH_FUZZYPID_H

include

include

include

enum fuzztab { NB = 0, NM = 1, NS = 2, ZO = 3, PS = 4, PM = 5, PB = 6, };

struct _MATH_FUZZYPID { / Property / float Property_Kp; float Property_Ki; float Property_Kd; 

  1. float Property_dt;
  3. float Property_integralMax;
  4. float Property_AimMax;
  5. float Property_OutputMax;
  7. float Value_errRange[7];
  8. float Value_errDotRange[7];
  9. float Value_deltaKpRange[7];
  10. float Value_deltaKiRange[7];
  11. float Value_deltaKdRange[7];
  13. /* Value */
  14. float Value_Aim;            
  15. float Value_Actual;         
  16. float Value_err;  
  17. float Value_errDot;     
  18. float Value_err_last;                            
  19. float Value_integral;
  21. float Value_output;
  23. rt_uint8_t Value_errBelongStage[2];
  24. float        Value_errBelongValue[2];
  25. rt_uint8_t Value_errDotBelongStage[2];
  26. float        Value_errDotBelongValue[2];
  28. float Value_deltaKp;
  29. float Value_deltaKi;
  30. float Value_deltaKd;
  32. /* Method */
  33. void (*Method_Init)(struct _MATH_FUZZYPID *module);
  34. void (*Method_Update)(struct _MATH_FUZZYPID *module,float Aim, float Feedback);

}; typedef struct _MATH_FUZZYPID MATH_FUZZYPID;

rt_err_t Module_FuzzyPID_Config(MATH_FUZZYPID *module);

endif

  1. <a name="H4nEk"></a>
  2. ## Math_FuzzyPID.c
  3. ```c
  4. /*
  5.  * Copyright (c) 2020 - ~, HIT_HERO Team
  6.  *
  7.  * FUZZYPID MODULE SOUCE FILE
  8.  * Used in RT-Thread Operate System
  9.  *
  10.  * Change Logs:
  11.  * Date           Author       Notes                Mail
  12.  * 2020-08-20     WangXi       first version        WangXi_chn@foxmail.com
  13.  * 
  14.  * Note:
  15.  * 
  16.  */
  17. #include "Math_FuzzyPID.h"
  18. #include <math.h>
  20. #define FUZZYPID
  22. enum fuzztab KPTAB[7][7] = 
  23. {
  24.     PB,PB,PM,PM,PS,ZO,ZO,
  25.     PB,PB,PM,PS,PS,ZO,NS,
  26.     PM,PM,PM,PS,ZO,NS,NS,
  27.     PM,PM,PS,ZO,NS,NM,NM,
  28.     PS,PS,ZO,NS,NS,NM,NM,
  29.     PS,ZO,NS,NM,NM,NM,NB,
  30.     ZO,ZO,NM,NM,NM,NB,NB
  31. };
  33. enum fuzztab KITAB[7][7] =
  34. {
  35.     NB,NB,NM,NM,NS,ZO,ZO,
  36.     NB,NB,NM,NS,NS,ZO,ZO,
  37.     NB,NM,NS,NS,ZO,PS,PS,
  38.     NM,NM,NS,ZO,PS,PM,PM,
  39.     NM,NS,ZO,PS,PS,PM,PB,
  40.     ZO,ZO,PS,PS,PM,PB,PB,
  41.     ZO,ZO,PS,PM,PM,PB,PB
  42. };
  44. enum fuzztab KDTAB[7][7] =
  45. {
  46.     PS,NS,NB,NB,NB,NM,PS,
  47.     PS,NS,NB,NM,NM,NS,ZO,
  48.     ZO,NS,NM,NM,NS,NS,ZO,
  49.     ZO,NS,NS,NS,NS,NS,ZO,
  50.     ZO,ZO,ZO,ZO,ZO,ZO,ZO,
  51.     PB,NS,PS,PS,PS,PS,PB,
  52.     PB,PM,PM,PM,PS,PS,PB
  53. };
  55. static void Module_FuzzyPIDInit(MATH_FUZZYPID *module);
  56. static void Module_FuzzyPIDUpdate(MATH_FUZZYPID *module,float Aim, float Feedback);
  57. static void fuzziFication(MATH_FUZZYPID *module);
  58. static void defuzzify(MATH_FUZZYPID *module);
  60. rt_err_t Module_FuzzyPID_Config(MATH_FUZZYPID *module)
  61. {
  62.     if( module->Method_Init        ==NULL &&
  63.         module->Method_Update    ==NULL 
  64.     ){  
  65.         /* Link the Method */
  66.         module->Method_Init     = Module_FuzzyPIDInit;
  67.         module->Method_Update     = Module_FuzzyPIDUpdate;    
  68.     }
  69.     else{
  70.         rt_kprintf("Warning: Module Fuzzy PID is Configed twice\n");
  71.         return RT_ERROR;
  72.     }
  74.     /* Device Init */
  75.     module->Method_Init(module);
  77.     return RT_EOK;
  78. }
  80. static void Module_FuzzyPIDInit(MATH_FUZZYPID *module)
  81. {
  82.     module->Value_Actual = 0;
  83.     module->Value_Aim = 0;
  84.     module->Value_err = 0;
  85.     module->Value_err_last = 0;
  86.     module->Value_integral = 0;
  87.     module->Value_output = 0;
  88. }
  90. static void Module_FuzzyPIDUpdate(MATH_FUZZYPID *module,float Aim, float Feedback)
  91. {
  92.     float P,I,D;
  94.     module->Value_Aim = Aim;
  95.     module->Value_Actual = Feedback;
  97.     /* Culculate P */    
  98.     if(module->Value_Aim > module->Property_AimMax)
  99.         module->Value_Aim = module->Property_AimMax;
  100.     else if(module->Value_Aim < -module->Property_AimMax)
  101.         module->Value_Aim = -module->Property_AimMax;
  103.     module->Value_err = module->Value_Aim - module->Value_Actual;
  104.     module->Value_errDot = module->Value_err - module->Value_err_last;
  105.     module->Value_err_last = module->Value_err;
  107. #ifdef FUZZYPID
  108.     /* Fuzzy PID Used */
  109.     fuzziFication(module);
  110.     defuzzify(module);
  111.     module->Property_Kp += module->Value_deltaKp;
  112.     module->Property_Ki += module->Value_deltaKi;
  113.     //module->Property_Kd += module->Value_deltaKd;
  114. #endif
  116.     P = module->Value_err * module->Property_Kp;
  118.     /* Culculate I */
  119.     if(fabs(module->Value_err) < 500)
  120.     {
  121.         module->Value_integral += module->Value_err * module->Property_Ki * module->Property_dt;
  123.         if(module->Value_integral > module->Property_integralMax)
  124.             module->Value_integral = module->Property_integralMax;
  125.         else if(module->Value_integral < -module->Property_integralMax)
  126.             module->Value_integral = -module->Property_integralMax;
  128.         I = module->Value_integral;
  129.     }
  131.     /* Culculate D */
  132.     D = module->Property_Kd * (module->Value_err - module->Value_err_last) / module->Property_dt;
  134.     /* Culculate Output */
  135.     module->Value_output = P + I + D;
  137.     if(module->Value_output > module->Property_OutputMax)
  138.         module->Value_output = module->Property_OutputMax;
  139.     else if(module->Value_output < -module->Property_OutputMax)
  140.         module->Value_output = -module->Property_OutputMax;
  141. }
  143. static void fuzziFication(MATH_FUZZYPID *module)
  144. {
  145.     /* get err fuzzy */
  146.     if(module->Value_err > module->Value_errRange[6])
  147.     {
  148.         module->Value_errBelongStage[0] = PB;
  149.         module->Value_errBelongStage[1] = PB;
  150.         module->Value_errBelongValue[0] = 1;
  151.         module->Value_errBelongValue[1] = 0;
  152.     }
  153.     else if(module->Value_err < module->Value_errRange[0])
  154.     {
  155.         module->Value_errBelongStage[1] = NB;
  156.         module->Value_errBelongStage[0] = NB;
  157.         module->Value_errBelongValue[0] = 0;
  158.         module->Value_errBelongValue[1] = 1;
  159.     }
  160.     else
  161.     {
  162.         for(rt_uint8_t i = 0;i<5;i++)
  163.         {
  164.             if(module->Value_err == module->Value_errRange[i])
  165.             {
  166.                 module->Value_errBelongStage[1] = i;
  167.                 module->Value_errBelongStage[0] = i;
  168.                 module->Value_errBelongValue[0] = 0;
  169.                 module->Value_errBelongValue[1] = 1;
  170.             }
  171.             else if((module->Value_err>module->Value_errRange[i])&&(module->Value_err<module->Value_errRange[i+1]))
  172.             {
  173.                 float step = fabs(module->Value_errRange[1]-module->Value_errRange[0]);
  174.                 module->Value_errBelongStage[0] = i;
  175.                 module->Value_errBelongStage[1] = i+1;
  176.                 module->Value_errBelongValue[0] = 
  177.                     fabs((module->Value_err-module->Value_errRange[i])/step);
  178.                 module->Value_errBelongValue[1] = 1-module->Value_errBelongValue[0];
  180.                 break;
  181.             }
  182.         }
  183.     }
  185.     /* get err last fuzzy */
  186.     if(module->Value_errDot > module->Value_errDotRange[6])
  187.     {
  188.         module->Value_errDotBelongStage[0] = PB;
  189.         module->Value_errDotBelongStage[1] = PB;
  190.         module->Value_errDotBelongValue[0] = 1;
  191.         module->Value_errDotBelongValue[1] = 0;
  192.     }
  193.     else if(module->Value_errDot < module->Value_errDotRange[0])
  194.     {
  195.         module->Value_errDotBelongStage[1] = NB;
  196.         module->Value_errDotBelongStage[0] = NB;
  197.         module->Value_errDotBelongValue[0] = 0;
  198.         module->Value_errDotBelongValue[1] = 1;
  199.     }
  200.     else
  201.     {
  202.         for(rt_uint8_t i = 0;i<5;i++)
  203.         {
  204.             if(module->Value_errDot == module->Value_errDotRange[i])
  205.             {
  206.                 module->Value_errDotBelongStage[1] = i;
  207.                 module->Value_errDotBelongStage[0] = i;
  208.                 module->Value_errDotBelongValue[0] = 0;
  209.                 module->Value_errDotBelongValue[1] = 1;
  210.             }
  211.             else if((module->Value_errDot>module->Value_errDotRange[i])&&(module->Value_errDot<module->Value_errDotRange[i+1]))
  212.             {
  213.                 float step = fabs(module->Value_errDotRange[1]-module->Value_errDotRange[0]);
  214.                 module->Value_errDotBelongStage[0] = i;
  215.                 module->Value_errDotBelongStage[1] = i+1;
  216.                 module->Value_errDotBelongValue[0] = 
  217.                     fabs((module->Value_err-module->Value_errRange[i])/step);
  218.                 module->Value_errDotBelongValue[1] = 1-module->Value_errDotBelongValue[0];
  220.                 break;
  221.             }
  222.         }
  223.     }
  224. }
  226. static void defuzzify(MATH_FUZZYPID *module)
  227. {
  228.     module->Value_deltaKp = 
  229.         module->Value_deltaKpRange[KPTAB[module->Value_errBelongStage[0]][module->Value_errDotBelongStage[0]]]
  230.             *module->Value_errBelongValue[0]*module->Value_errDotBelongValue[0]+
  231.         module->Value_deltaKpRange[KPTAB[module->Value_errBelongStage[0]][module->Value_errDotBelongStage[1]]]
  232.             *module->Value_errBelongValue[0]*module->Value_errDotBelongValue[1]+
  233.         module->Value_deltaKpRange[KPTAB[module->Value_errBelongStage[1]][module->Value_errDotBelongStage[0]]]
  234.             *module->Value_errBelongValue[1]*module->Value_errDotBelongValue[0]+
  235.         module->Value_deltaKpRange[KPTAB[module->Value_errBelongStage[1]][module->Value_errDotBelongStage[1]]]
  236.             *module->Value_errBelongValue[1]*module->Value_errDotBelongValue[1];
  238.     module->Value_deltaKi = 
  239.         module->Value_deltaKiRange[KITAB[module->Value_errBelongStage[0]][module->Value_errDotBelongStage[0]]]
  240.             *module->Value_errBelongValue[0]*module->Value_errDotBelongValue[0]+
  241.         module->Value_deltaKiRange[KITAB[module->Value_errBelongStage[0]][module->Value_errDotBelongStage[1]]]
  242.             *module->Value_errBelongValue[0]*module->Value_errDotBelongValue[1]+
  243.         module->Value_deltaKiRange[KITAB[module->Value_errBelongStage[1]][module->Value_errDotBelongStage[0]]]
  244.             *module->Value_errBelongValue[1]*module->Value_errDotBelongValue[0]+
  245.         module->Value_deltaKiRange[KITAB[module->Value_errBelongStage[1]][module->Value_errDotBelongStage[1]]]
  246.             *module->Value_errBelongValue[1]*module->Value_errDotBelongValue[1];
  248.     module->Value_deltaKd = 
  249.         module->Value_deltaKdRange[KDTAB[module->Value_errBelongStage[0]][module->Value_errDotBelongStage[0]]]
  250.             *module->Value_errBelongValue[0]*module->Value_errDotBelongValue[0]+
  251.         module->Value_deltaKdRange[KDTAB[module->Value_errBelongStage[0]][module->Value_errDotBelongStage[1]]]
  252.             *module->Value_errBelongValue[0]*module->Value_errDotBelongValue[1]+
  253.         module->Value_deltaKdRange[KDTAB[module->Value_errBelongStage[1]][module->Value_errDotBelongStage[0]]]
  254.             *module->Value_errBelongValue[1]*module->Value_errDotBelongValue[0]+
  255.         module->Value_deltaKdRange[KDTAB[module->Value_errBelongStage[1]][module->Value_errDotBelongStage[1]]]
  256.             *module->Value_errBelongValue[1]*module->Value_errDotBelongValue[1];
  257. }

Module_DjiC610C620Group.h

  1. 修改电机的PID成员为模糊PID
  2. struct _MODULE_DjiC610620
  3. {
  4.     /* Property */
  5.     rt_uint8_t         Enable;
  6.     rt_uint8_t        Mode;
  8.     MATH_FUZZYPID     PID_Speed;
  9.     MATH_FUZZYPID     PID_Angle;

Module_DjiC610C620Group.c

  1. 在电机群的电机成员初始化时，把原来对普通PID的初始化修改为对模糊PID的初始化
  2. /* Module PID controler Init */
  3.     if(Dev_DjiC610620->Mode == DjiC610620MODE_SPEED)
  4.     {
  5.         Module_FuzzyPID_Config(&(Dev_DjiC610620->PID_Speed));
  6.     }
  7.     else if(Dev_DjiC610620->Mode == DjiC610620MODE_ANGLE)
  8.     {
  9.         Module_FuzzyPID_Config(&(Dev_DjiC610620->PID_Speed));
  10.         Module_FuzzyPID_Config(&(Dev_DjiC610620->PID_Angle));
  11.     }

因为PID参数更新的方法没有改变绑定关系，因此无需变动

main.c

和原来相比在main中电机群模块的初始化多了对模糊PID参数的初始化，主要是模糊范围

  1. MODULE_DjiC610620GROUP dev_DjiC610620group = 
  2. {
  3.     .Property_CanDevName = "can1",
  4.     .Value_module_DjiC610620[DjiC610620ID_1] = 
  5.     {
  6.         .Enable = 1,
  7.         .Mode    = DjiC610620MODE_SPEED,
  8.         .PID_Speed = 
  9.         {
  10.             .Property_Kp = 6,    .Property_Ki = 2,    .Property_Kd = 0,
  11.             .Value_errRange     = {-6750,    -4500,    -2250,    0,        2250,    4500,    6750},
  12.             .Value_errDotRange  = {-13500,    -9000,    -4500,    0,        4500,    9000,    13500},
  13.             .Value_deltaKpRange = {-0.015,    -0.01,    -0.005,    0,    0.005,    0.01,    0.015},
  14.             .Value_deltaKiRange = {-0.003,    -0.002,    -0.001,    0,    0.001,    0.002,    0.003},
  15.             .Value_deltaKdRange = {-0.015,    -0.01,    -0.005,    0,    0.005,    0.01,    0.015},
  16.             .Property_dt = 0.005,
  17.             .Property_integralMax = 500,    .Property_AimMax = 9000,    .Property_OutputMax =16300            
  18.         }
  19.     },
  20. };

实际效果

  • 电机的PID控制器中Kp、Ki可以实现闭环调整，在速度环运行过程中自动寻找Kp、Ki的最佳值
  • 表现形式是电机在加速过程中Kp逐渐上升，直至电机抖动，然后Kp自动回落，电机平稳，往复进行
  • 据此猜测模糊PID的适用场合
    • 模糊PID和传统PID的优点在于无需给定明确的参数值，只需给定一个增量的调整范围
    • 可以实现参数的自适应，此时的PID更像是一个学习过程
    • 因为是学习过程，因此存在学习时间，可能不适合应用在实时性要求严格的场合
    • 可以用来寻找系统的最佳的参数，从而成为一个工具