c# 实现模糊PID控制算法

跑起来的效果看每个类的test方法，自己调用来测试

目的是看看哪个算法好用，移植的时候比较单纯没有研究懂算法，代码结构也没改动，只是移植到C#方便查看代码和测试，大家要拷贝也很方便，把整个类拷贝到.cs文件即可

这段算法在实际值低于目标值是工作正常，超过后会有问题，不知道如何调教

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace FuzzyPID
{
 class FuzzyPID
 {
   public const int N = 7;

double target; //系统的控制目标
   double actual; //采样获得的实际值
   double e; //误差
   double e_pre_1; //上一次的误差
   double e_pre_2; //上上次的误差
   double de;   //误差的变化率
   double emax;  //误差基本论域上限
   double demax;  //误差辩化率基本论域的上限
   double delta_Kp_max;  //delta_kp输出的上限
   double delta_Ki_max;  //delta_ki输出上限
   double delta_Kd_max;  //delta_kd输出上限
   double Ke;   //Ke=n/emax,量化论域为[-3,-2,-1,0,1,2,3]
   double Kde;   //Kde=n/demax,量化论域为[-3,-2,-1,0,1,2,3]
   double Ku_p;  //Ku_p=Kpmax/n,量化论域为[-3,-2,-1,0,1,2,3]
   double Ku_i;  //Ku_i=Kimax/n,量化论域为[-3,-2,-1,0,1,2,3]
   double Ku_d;  //Ku_d=Kdmax/n,量化论域为[-3,-2,-1,0,1,2,3]
   int[,] Kp_rule_matrix = new int[N, N];//Kp模糊规则矩阵
   int[,] Ki_rule_matrix = new int[N, N];//Ki模糊规则矩阵
   int[,] Kd_rule_matrix = new int[N, N];//Kd模糊规则矩阵
   string mf_t_e;    //e的隶属度函数类型
   string mf_t_de;   //de的隶属度函数类型
   string mf_t_Kp;   //kp的隶属度函数类型
   string mf_t_Ki;   //ki的隶属度函数类型
   string mf_t_Kd;   //kd的隶属度函数类型
   double[] e_mf_paras; //误差的隶属度函数的参数
   double[] de_mf_paras;//误差的偏差隶属度函数的参数
   double[] Kp_mf_paras; //kp的隶属度函数的参数
   double[] Ki_mf_paras; //ki的隶属度函数的参数
   double[] Kd_mf_paras; //kd的隶属度函数的参数
   double Kp;
   double Ki;
   double Kd;
   double A;
   double B;
   double C;

public FuzzyPID(double e_max, double de_max, double kp_max, double ki_max, double kd_max, double Kp0, double Ki0, double Kd0)
   {
     emax = e_max;
     demax = de_max;
     delta_Kp_max = kp_max;
     delta_Ki_max = ki_max;
     delta_Kd_max = kd_max;
     e = target - actual;
     de = e - e_pre_1;
     Ke = (N / 2) / emax;
     Kde = (N / 2) / demax;
     Ku_p = delta_Kp_max / (N / 2);
     Ku_i = delta_Ki_max / (N / 2);
     Ku_d = delta_Kd_max / (N / 2);
     Kp = Kp0;
     Ki = Ki0;
     Kd = Kd0;
     A = Kp + Ki + Kd;
     B = -2 * Kd - Kp;
     C = Kd;
   }

//三角隶属度函数
   double trimf(double x, double a, double b, double c)
   {
     double u;
     if (x >= a && x <= b)
       u = (x - a) / (b - a);
     else if (x > b && x <= c)
       u = (c - x) / (c - b);
     else
       u = 0;
     return u;
   }

//正态隶属度函数
   double gaussmf(double x, double ave, double sigma)
   {
     double u;
     if (sigma < 0)
     {
       throw new Exception("In gaussmf, sigma must larger than 0");
     }
     u = Math.Exp(-Math.Pow(((x - ave) / sigma), 2));
     return u;
   }

//梯形隶属度函数
   double trapmf(double x, double a, double b, double c, double d)
   {
     double u;
     if (x >= a && x < b)
       u = (x - a) / (b - a);
     else if (x >= b && x < c)
       u = 1;
     else if (x >= c && x <= d)
       u = (d - x) / (d - c);
     else
       u = 0;
     return u;
   }

//设置模糊规则Matrix
   public void setRuleMatrix(int[,] kp_m, int[,] ki_m, int[,] kd_m)
   {
     for (int i = 0; i < N; i++)
       for (int j = 0; j < N; j++)
       {
         Kp_rule_matrix[i, j] = kp_m[i, j];
         Ki_rule_matrix[i, j] = ki_m[i, j];
         Kd_rule_matrix[i, j] = kd_m[i, j];
       }

}

//设置模糊隶属度函数的子函数
   void setMf_sub(string type, double[] paras, int n)
   {
     int N_mf_e = 0, N_mf_de = 0, N_mf_Kp = 0, N_mf_Ki = 0, N_mf_Kd = 0;
     switch (n)
     {
       case 0:
         if (type == "trimf" || type == "gaussmf" || type == "trapmf")
           mf_t_e = type;
         else
           throw new Exception("Type of membership function must be \"trimf\" or \"gaussmf\" or \"trapmf\"");
         if (mf_t_e == "trimf")
           N_mf_e = 3;
         else if (mf_t_e == "gaussmf")
           N_mf_e = 2;
         else if (mf_t_e == "trapmf")
           N_mf_e = 4;

e_mf_paras = new double[N * N_mf_e];
         for (int i = 0; i < N * N_mf_e; i++)
           e_mf_paras[i] = paras[i];
         break;

case 1:
         if (type == "trimf" || type == "gaussmf" || type == "trapmf")
           mf_t_de = type;
         else
           throw new Exception("Type of membership function must be \"trimf\" or \"gaussmf\" or \"trapmf\"");
         if (mf_t_de == "trimf")
           N_mf_de = 3;
         else if (mf_t_de == "gaussmf")
           N_mf_de = 2;
         else if (mf_t_de == "trapmf")
           N_mf_de = 4;
         de_mf_paras = new double[N * N_mf_de];
         for (int i = 0; i < N * N_mf_de; i++)
           de_mf_paras[i] = paras[i];
         break;

case 2:
         if (type == "trimf" || type == "gaussmf" || type == "trapmf")
           mf_t_Kp = type;
         else
           throw new Exception("Type of membership function must be \"trimf\" or \"gaussmf\" or \"trapmf\"");
         if (mf_t_Kp == "trimf")
           N_mf_Kp = 3;
         else if (mf_t_Kp == "gaussmf")
           N_mf_Kp = 2;
         else if (mf_t_Kp == "trapmf")
           N_mf_Kp = 4;
         Kp_mf_paras = new double[N * N_mf_Kp];
         for (int i = 0; i < N * N_mf_Kp; i++)
           Kp_mf_paras[i] = paras[i];
         break;

case 3:
         if (type == "trimf" || type == "gaussmf" || type == "trapmf")
           mf_t_Ki = type;
         else
           throw new Exception("Type of membership function must be \"trimf\" or \"gaussmf\" or \"trapmf\"");
         if (mf_t_Ki == "trimf")
           N_mf_Ki = 3;
         else if (mf_t_Ki == "gaussmf")
           N_mf_Ki = 2;
         else if (mf_t_Ki == "trapmf")
           N_mf_Ki = 4;
         Ki_mf_paras = new double[N * N_mf_Ki];
         for (int i = 0; i < N * N_mf_Ki; i++)
           Ki_mf_paras[i] = paras[i];
         break;

case 4:
         if (type == "trimf" || type == "gaussmf" || type == "trapmf")
           mf_t_Kd = type;
         else
           throw new Exception("Type of membership function must be \"trimf\" or \"gaussmf\" or \"trapmf\"");
         if (mf_t_Kd == "trimf")
           N_mf_Kd = 3;
         else if (mf_t_Kd == "gaussmf")
           N_mf_Kd = 2;
         else if (mf_t_Kd == "trapmf")
           N_mf_Kd = 4;
         Kd_mf_paras = new double[N * N_mf_Kd];
         for (int i = 0; i < N * N_mf_Kd; i++)
           Kd_mf_paras[i] = paras[i];
         break;

default: break;
     }
   }

//设置模糊隶属度函数的类型和参数
   public void setMf(string mf_type_e, double[] e_mf,
      string mf_type_de, double[] de_mf,
      string mf_type_Kp, double[] Kp_mf,
      string mf_type_Ki, double[] Ki_mf,
      string mf_type_Kd, double[] Kd_mf)
   {
     setMf_sub(mf_type_e, e_mf, 0);
     setMf_sub(mf_type_de, de_mf, 1);
     setMf_sub(mf_type_Kp, Kp_mf, 2);
     setMf_sub(mf_type_Ki, Ki_mf, 3);
     setMf_sub(mf_type_Kd, Kd_mf, 4);
   }

//实现模糊控制
   public double realize(double t, double a)
   {
     double[] u_e = new double[N],
       u_de = new double[N],
       u_u = new double[N];
     int[] u_e_index = new int[3], u_de_index = new int[3];//假设一个输入最多激活3个模糊子集
     double delta_Kp, delta_Ki, delta_Kd;
     double delta_u;
     target = t;
     actual = a;
     e = target - actual;
     de = e - e_pre_1;
     e = Ke * e;
     de = Kde * de;
     /* 将误差e模糊化*/
     int j = 0;
     for (int i = 0; i < N; i++)
     {
       if (mf_t_e == "trimf")
         u_e[i] = trimf(e, e_mf_paras[i * 3], e_mf_paras[i * 3 + 1], e_mf_paras[i * 3 + 2]);//e模糊化，计算它的隶属度
       else if (mf_t_e == "gaussmf")
         u_e[i] = gaussmf(e, e_mf_paras[i * 2], e_mf_paras[i * 2 + 1]);//e模糊化，计算它的隶属度
       else if (mf_t_e == "trapmf")
         u_e[i] = trapmf(e, e_mf_paras[i * 4], e_mf_paras[i * 4 + 1], e_mf_paras[i * 4 + 2], e_mf_paras[i * 4 + 3]);//e模糊化，计算它的隶属度

if (u_e[i] != 0)
         u_e_index[j++] = i;        //存储被激活的模糊子集的下标，可以减小计算量
     }
     for (; j < 3; j++) u_e_index[j] = 0;       //富余的空间填0

/*将误差变化率de模糊化*/
     j = 0;
     for (int i = 0; i < N; i++)
     {
       if (mf_t_de == "trimf")
         u_de[i] = trimf(de, de_mf_paras[i * 3], de_mf_paras[i * 3 + 1], de_mf_paras[i * 3 + 2]);//de模糊化，计算它的隶属度
       else if (mf_t_de == "gaussmf")
         u_de[i] = gaussmf(de, de_mf_paras[i * 2], de_mf_paras[i * 2 + 1]);//de模糊化，计算它的隶属度
       else if (mf_t_de == "trapmf")
         u_de[i] = trapmf(de, de_mf_paras[i * 4], de_mf_paras[i * 4 + 1], de_mf_paras[i * 4 + 2], de_mf_paras[i * 4 + 3]);//de模糊化，计算它的隶属度

if (u_de[i] != 0)
         u_de_index[j++] = i;      //存储被激活的模糊子集的下标，可以减小计算量
     }
     for (; j < 3; j++) u_de_index[j] = 0;     //富余的空间填0

double den = 0, num = 0;
     /*计算delta_Kp和Kp*/
     for (int m = 0; m < 3; m++)
       for (int n = 0; n < 3; n++)
       {
         num += u_e[u_e_index[m]] * u_de[u_de_index[n]] * Kp_rule_matrix[u_e_index[m], u_de_index[n]];
         den += u_e[u_e_index[m]] * u_de[u_de_index[n]];
       }
     delta_Kp = num / den;
     delta_Kp = Ku_p * delta_Kp;
     if (delta_Kp >= delta_Kp_max) delta_Kp = delta_Kp_max;
     else if (delta_Kp <= -delta_Kp_max) delta_Kp = -delta_Kp_max;
     Kp += delta_Kp;
     if (Kp < 0) Kp = 0;
     /*计算delta_Ki和Ki*/
     den = 0; num = 0;
     for (int m = 0; m < 3; m++)
       for (int n = 0; n < 3; n++)
       {
         num += u_e[u_e_index[m]] * u_de[u_de_index[n]] * Ki_rule_matrix[u_e_index[m], u_de_index[n]];
         den += u_e[u_e_index[m]] * u_de[u_de_index[n]];
       }

delta_Ki = num / den;
     delta_Ki = Ku_i * delta_Ki;
     if (delta_Ki >= delta_Ki_max) delta_Ki = delta_Ki_max;
     else if (delta_Ki <= -delta_Ki_max) delta_Ki = -delta_Ki_max;
     Ki += delta_Ki;
     if (Ki < 0) Ki = 0;
     /*计算delta_Kd和Kd*/
     den = 0; num = 0;
     for (int m = 0; m < 3; m++)
       for (int n = 0; n < 3; n++)
       {
         num += u_e[u_e_index[m]] * u_de[u_de_index[n]] * Kd_rule_matrix[u_e_index[m], u_de_index[n]];
         den += u_e[u_e_index[m]] * u_de[u_de_index[n]];
       }
     delta_Kd = num / den;
     delta_Kd = Ku_d * delta_Kd;
     if (delta_Kd >= delta_Kd_max) delta_Kd = delta_Kd_max;
     else if (delta_Kd <= -delta_Kd_max) delta_Kd = -delta_Kd_max;
     Kd += delta_Kd;
     if (Kd < 0) Kd = 0;

A = Kp + Ki + Kd;
     B = -2 * Kd - Kp;
     C = Kd;
     delta_u = A * e + B * e_pre_1 + C * e_pre_2;

delta_u = delta_u / Ke;

if (delta_u >= 0.95 * target) delta_u = 0.95 * target;
     else if (delta_u <= -0.95 * target) delta_u = -0.95 * target;

e_pre_2 = e_pre_1;
     e_pre_1 = e;

return delta_u;
   }

void showMf(string type, double[] mf_paras)
   {
     int tab = 0;
     if (type == "trimf")
       tab = 2;
     else if (type == "gaussmf")
       tab = 1;
     else if (type == "trapmf")
       tab = 3;
     this.WriteLine($"函数类型：{mf_t_e}");
     this.WriteLine("函数参数列表：");
     double[] p = mf_paras;
     for (int i = 0; i < N * (tab + 1); i++)
     {
       this.Write(p[i] + " ");
       if (i ％ (tab + 1) == tab)
         this.Write("\r\n");
     }
   }

public void showInfo()
   {
     this.WriteLine("Info of this fuzzy controller is as following:");
     this.WriteLine($"基本论域e：[{-emax},{emax}]");
     this.WriteLine($"基本论域de：[{-demax},{demax}]");
     this.WriteLine($"基本论域delta_Kp：[{-delta_Kp_max},{delta_Kp_max}]");
     this.WriteLine($"基本论域delta_Ki：[{-delta_Ki_max},{delta_Ki_max}]");
     this.WriteLine($"基本论域delta_Kd：[{-delta_Kd_max},{delta_Kd_max}]");
     this.WriteLine("误差e的模糊隶属度函数参数：");
     showMf(mf_t_e, e_mf_paras);
     this.WriteLine("误差变化率de的模糊隶属度函数参数：");
     showMf(mf_t_de, de_mf_paras);
     this.WriteLine("delta_Kp的模糊隶属度函数参数：");
     showMf(mf_t_Kp, Kp_mf_paras);
     this.WriteLine("delta_Ki的模糊隶属度函数参数：");
     showMf(mf_t_Ki, Ki_mf_paras);
     this.WriteLine("delta_Kd的模糊隶属度函数参数：");
     showMf(mf_t_Kd, Kd_mf_paras);
     this.WriteLine("模糊规则表：");
     this.WriteLine("delta_Kp的模糊规则矩阵");
     for (int i = 0; i < N; i++)
     {
       for (int j = 0; j < N; j++)
       {
         this.Write(Kp_rule_matrix[i, j]);
       }
       this.Write("\r\n");
     }
     this.WriteLine("delta_Ki的模糊规则矩阵"); ;
     for (int i = 0; i < N; i++)
     {
       for (int j = 0; j < N; j++)
       {
         this.WriteLine(Ki_rule_matrix[i, j]);
       }
       WriteEnd();
     }
     this.WriteLine("delta_Kd的模糊规则矩阵"); ;
     for (int i = 0; i < N; i++)
     {
       for (int j = 0; j < N; j++)
       {
         this.WriteLine(Kd_rule_matrix[i, j]);
       }
       WriteEnd();
     }
     this.WriteLine($"误差的量化比例因子Ke={Ke}");
     this.WriteLine($"误差变化率的量化比例因子Kde={Kde}");
     this.WriteLine($"输出的量化比例因子Ku_p={Ku_p}");
     this.WriteLine($"输出的量化比例因子Ku_i={Ku_i}");
     this.WriteLine($"输出的量化比例因子Ku_d={Ku_d}");
     this.WriteLine($"设定目标target={target}");
     this.WriteLine($"误差e={e}");
     this.WriteLine($"Kp={Kp}");
     this.WriteLine($"Ki={Ki}");
     this.WriteLine($"Kd={Kd}");
     WriteEnd();
   }

public void Write(object str)
   {
     Console.Write(str);
   }
   public void WriteLine(object str)
   {
     Console.WriteLine(str);
   }
   public void WriteEnd()
   {
     Console.Write("\r\n");
   }

public static void test()
   {
     int NB = -3;
     int NM = -2;
     int NS = -1;
     int ZO = 0;
     int PS = 1;
     int PM = 2;
     int PB = 3;

double target = 300;
     double actual = 400;
     double u = 0;

int[,] deltaKpMatrix = new int[7, 7] {{PB,PB,PM,PM,PS,ZO,ZO
 },
              {PB,PB,PM,PS,PS,ZO,NS
},
              {PM,PM,PM,PS,ZO,NS,NS},
              {PM,PM,PS,ZO,NS,NM,NM},
              {PS,PS,ZO,NS,NS,NM,NM},
              {PS,ZO,NS,NM,NM,NM,NB},
              {ZO,ZO,NM,NM,NM,NB,NB}};
     int[,] deltaKiMatrix = new int[7, 7]{{NB,NB,NM,NM,NS,ZO,ZO},
              {NB,NB,NM,NS,NS,ZO,ZO},
              {NB,NM,NS,NS,ZO,PS,PS},
              {NM,NM,NS,ZO,PS,PM,PM},
              {NM,NS,ZO,PS,PS,PM,PB},
              {ZO,ZO,PS,PS,PM,PB,PB},
              {ZO,ZO,PS,PM,PM,PB,PB}};
     int[,] deltaKdMatrix = new int[7, 7]{{PS,NS,NB,NB,NB,NM,PS},
              {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}};
     double[] e_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
     double[] de_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
     double[] Kp_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
     double[] Ki_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };
     double[] Kd_mf_paras = { -3, -3, -2, -3, -2, -1, -2, -1, 0, -1, 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 3 };

var fuzzypid = new FuzzyPID(1500, 1000, 0.3, 0.9, 0.6, 0.01, 0.04, 0.01);
     fuzzypid.setMf("trimf", e_mf_paras, "trimf", de_mf_paras, "trimf", Kp_mf_paras, "trimf", Ki_mf_paras, "trimf", Kd_mf_paras);
     fuzzypid.setRuleMatrix(deltaKpMatrix, deltaKiMatrix, deltaKdMatrix);

for (int i = 0; i < 50; i++)
     {
       u = fuzzypid.realize(target, actual);
       actual += u;
       Console.WriteLine($"{i}  {target} {u} {actual}");

//if (i>19)
       //{
       //  target = 300;
       //}
     }
     //fuzzypid.showInfo();

}

}
}

来源：https://www.cnblogs.com/gxrsprite/p/12155890.html