fuzzy.cpp

00001 /************************************************************************************
00002 * @author：
00003 * @date : 2019/3/25
00004 * @fuction name：FUZZY_CONTROL
00005 * @fuction description： 模糊自适应控制算法
00006 *************************************************************************************/
00007 #include "fuzzy.h"
00008 
00009 fuzzy_control_t ankleFuzzy, kneeFuzzy;
00010 
00011 
00012 //模糊集合
00013 /*
00014 #define NL   -3
00015 #define NM   -2
00016 #define NS   -1
00017 #define ZE   0
00018 #define PS   1
00019 #define PM   2
00020 #define PL   3
00021 */
00022 const float FuzzyRuleKp[7][7]={
00023     /*                  EC                     */
00024     /*  -3,       -2,     -1,     0,     1,      2,     3 */
00025 /*-3*/ 473.86, 473.86, 416.63, 416.63, 333.37, 250.00, 250.00,
00026 /*-2*/ 473.86, 473.86, 416.63, 333.37, 333.37, 250.00, 250.00,
00027 /*-1*/ 416.63, 416.63, 416.63, 416.63, 250.00, 166.64, 166.64,
00028 /* 0*/ 416.63, 416.63, 416.63, 250.00, 333.37, 83.38,  83.38,
00029 /* 1*/ 333.37, 333.37, 250.00, 166.64, 166.64, 83.38,  83.38,
00030 /* 2*/ 333.37, 250.00, 166.64, 83.38,  83.38,  83.38,  26.15,
00031 /* 3*/ 250.00, 250.00, 83.38,  83.38,  83.38,  26.15,  26.15,
00032 };
00033 
00034 const float FuzzyRuleKd[7][7]={
00035     /*                  EC                     */
00036     /*  -3,   -2,   -1,   0,     1,   2,    3 */
00037 /*-3*/ 3.33, 1.67, 0.26, 0.26, 0.26, 0.83, 3.33,
00038 /*-2*/ 3.33, 2.50, 0.26, 0.26, 0.26, 0.83, 3.33,
00039 /*-1*/ 2.50, 1.67, 1.67, 0.83, 1.67, 1.67, 2.50,
00040 /* 0*/ 2.50, 1.67, 4.17, 1.67, 1.67, 1.67, 2.50,
00041 /* 1*/ 2.50, 2.50, 2.50, 2.50, 2.50, 2.50, 2.50,
00042 /* 2*/ 4.74, 1.67, 3.33, 3.33, 3.33, 3.33, 4.74,
00043 /* 3*/ 4.74, 4.17, 4.17, 4.17, 3.33, 3.33, 0.26,
00044 };
00045 
00046 
00047 void fuzzy_init(fuzzy_control_t *fuzzy ,float max_e,float max_ec,float gain)
00048 {
00049     fuzzy->quantied_e = 3.0f/max_e;
00050     fuzzy->quantied_ec = 3.0f/max_ec;
00051     fuzzy->quantied_resault = gain;
00052     fuzzy->outKp = 0.0f;
00053     fuzzy->outKd = 0.0f;
00054 }
00055 void fuzzy_control(float e,float ec,fuzzy_control_t *fuzzy,const float fuzzyRuleKp[7][7], const float fuzzyRuleKd[7][7])
00056 {
00057 
00058      float etemp,ectemp;
00059      float eLefttemp,ecLefttemp;
00060      float eRighttemp ,ecRighttemp;
00061 
00062      int eLeftIndex,ecLeftIndex;
00063      int eRightIndex,ecRightIndex;
00064      e = e * fuzzy->quantied_e;
00065      ec = ec * fuzzy->quantied_ec;
00066      e = range(e,-3.0f,3.0f);
00067      ec = range(ec,-3.0f,3.0f);
00068      etemp = e > 3.0f ? 0.0f : (e < - 3.0f ? 0.0f : (e >= 0.0f ? (e >= 2.0f ? 2.5f: (e >= 1.0f ? 1.5f : 0.5f)) : (e >= -1.0f ? -0.5f : (e >= -2.0f ? -1.5f : (e >= -3.0f ? -2.5f : 0.0f)))));
00069      eLeftIndex = (int)((etemp-0.5f) + 3.0f);        //[-3,3] -> [0,6]
00070      eRightIndex = (int)((etemp+0.5f) + 3.0f);
00071      eLefttemp =etemp == 0.0f ? 0.0f:((etemp+0.5f)-e);             
00072      eRighttemp=etemp == 0.0f ? 0.0f:( e-(etemp-0.5f));
00073      
00074      ectemp = ec > 3.0f ? 0.0f : (ec < - 3.0f ? 0.0f : (ec >= 0.0f ? (ec >= 2.0f ? 2.5f: (ec >= 1.0f ? 1.5f : 0.5f)) : (ec >= -1.0f ? -0.5f : (ec >= -2.0f ? -1.5f : (ec >= -3.0f ? -2.5f : 0.0f) ))));
00075      ecLeftIndex = (int)((ectemp-0.5f) + 3.0f);        //[-6,6] -> [0,12]
00076      ecRightIndex = (int)((ectemp+0.5f) + 3.0f);
00077      ecLefttemp =ectemp == 0.0f ? 0.0f:((ectemp+0.5f)-ec);
00078      ecRighttemp=ectemp == 0.0f ? 0.0f:( ec-(ectemp-0.5f));
00079 
00080 
00081 /*************************************反模糊*************************************/
00082 
00083 
00084 
00085 
00086     fuzzy->outKp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex]
00087                     + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex]
00088                     + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex]
00089                     + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);
00090                     
00091     fuzzy->outKd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex]
00092                     + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex]
00093                     + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex]
00094                     + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);
00095 
00096     //对解算出的模糊结果进行量化
00097     fuzzy->outKp = fuzzy->outKp * fuzzy->quantied_resault;
00098     fuzzy->outKd = fuzzy->outKd * fuzzy->quantied_resault;
00099 }