Arduino智能小车直线控制-模糊PID控制

最新推荐文章于 2024-07-24 11:32:27 发布

qq_33276575

最新推荐文章于 2024-07-24 11:32:27 发布

阅读量1.1w

点赞数 25

文章标签： arduino c++ pid

本文链接：https://blog.csdn.net/qq_33276575/article/details/105632129

版权

本文主要是基于Arduino智能小车直线走不直的问题，运用模糊PID算法进行控制。本文主要由三部分构成：模糊PID控制器的原理，模糊PID控制器Ｃ＋＋的实现与测试，把模糊PID控制算法应用于Arduino 智能小车走直线的过程中。

一，模糊PID原理

模糊PID控制流程如下图所示，把目标值 Xtarget 与输出值 Xout 的误差 e 与 e 的变化率 de/dt 作为模糊控制器的输入，模糊控制器先对输入进行模糊化处理，接着进行模糊推理，最后把模糊推理的结果进行去模糊处理输出PID控制器的三个参数 kp, ki, kd，从而达到对PID控制器参数自适应整定的效果。

根据以上的描述可知，模糊控制器主要由去模糊化，模糊推理以及去模糊三部分组成。以下将对三部分进行详细讲解。

1.1 模糊化

要实现模糊化首先需要对模糊化进行初始化，初始化包括论域的确定以及隶属度函数的确定。

1.1.1 论域

论域可以说是一个人为确定的范围，由于输入e,de/dt，输出kp.ki,kd的范围各不相同，把输入映射到论域上更好统一处理。确定论域的范围后，需要对论域进行模糊分类，模糊分类即对论域进行划分。假设论域的范围为[-3，3]，把论域平均分为5等份，即[-3,-2],[-2,-1],[-1,0],[0,1],[1,2],[2,3]。接着把每个端点进行等级划分，依次为-3--->NB(负大),-2--->NM(负中),-1---->NS(负小),0--->ZO(零),1---->PS(正小),2---->PM(正中),3---->PB(正大)。假设输入e的范围为[-10,10]，此刻e的值为8，通过映射后的值为2.4，2.4在[2,3]区间，则该点在正中与正大之间。示意图如下：

1.1.2 隶属度函数的确定。

常见的隶属度函数有三角形隶属度函数，梯形隶属度函数，抛物线型隶属度函数。以最简单的三角性隶属度函数为例，形状如下图所示：

由上图可知，隶属度函数的值域为[0,1]。若输入e经过映射后的值为2.4，那么对应下图红线与绿线的值分别为0.6，0.4,这两个就是隶属度，表示该输入属于PM的概率为0.6，而属于PB的概率为0.4。所以隶属度也为概率。

由上述论域和隶属度函数的讲解，可总结出模糊化的过程：区间映射，根据隶属度函数计算隶属度，流程图如下：

1 2. 模糊推理

模糊推理，即根据e与de/dt 的隶属度进行查表得到输出的大小程度，即NB，NS等。所以模糊推理的核心工作是建立推理表。其中模糊PID常用的推理表如下图所示：

以下以一个例子说明规则表的使用方法。

假设此刻的输入e为8，de/dt为-12，而e的范围为[-10,10]，de/dt的范围为[-20,20]。则通过模糊化得到e的隶属度为0.6(PM)与0.4(PB)，de/dt的隶属度为0.8(NM)与0.2(NS)，然后，对e与de/dt的隶属度进行两两组合，并进行查表，得到下表的关系：

接着，计算各输出Kp,Ki,Kd的隶属度。

Kp:

同理Ki.Kd也计算隶属度。

最后进行个输出的隶属度进行整合，例如Kp,由上面计算可知，Kp的隶属度为0.8(ZO)，0.12(NS)，0.08(NM)。

1.3 去模糊

去模糊是根据模糊推理得到的各输出的隶属度算出输出在论域中的哪个值，然后根据区间映射关系，得到输出。

1.3.1 计算输出在论域中的值

以上面的例子进行阐述计算的过程。由上面可知，Kp的隶属度为0.8(ZO)，0.12(NS)，0.08(NM),而在论域讲解时，已经将ZO的值定为0，NS的值定为-1，NM的值定为-2。那么Kp的期望为：

把期望作为Kp在论域的值，在确定Kp的范围后，根据区间映射公式，可得出Kp的输出值。

以上为模糊控制器的流程。值得注意的是，输出的Kp,Ki,Kd为增量。在初始化时要确定输入与输出的范围（区间），用于进行区间映射。

2.1 C++实现模糊PID控制器

该本版隶属度函数为固定三角形隶属度函数，论域固定为[-3,3]。

FuzzyPID.h头文件

#ifndef FuzzyPID_H
#define FuzzyPID_H
class FuzzyPID
{
public:
	FuzzyPID();
	~FuzzyPID();
	void Get_grad_membership(float erro, float erro_c);
	float Quantization(float maximum, float minimum, float x);
	float Inverse_quantization(float maximum, float minimum, float qvalues);
	void GetSumGrad();
	void GetOUT();
	float FuzzyPIDcontroller(float e_max, float e_min, float ec_max, float ec_min, float kp_max, float kp_min, float erro, float erro_c, float ki_max, float ki_min,float kd_max, float kd_min,float erro_pre, float errp_ppre);
	const int  num_area = 8; //划分区域个数
	//float e_max;  //误差做大值
	//float e_min;  //误差最小值
	//float ec_max;  //误差变化最大值
	//float ec_min;  //误差变化最小值
	//float kp_max, kp_min;
	float e_membership_values[7] = {-3,-2,-1,0,1,2,3}; //输入e的隶属值
	float ec_membership_values[7] = { -3,-2,-1,0,1,2,3 };//输入de/dt的隶属值
	float kp_menbership_values[7] = { -3,-2,-1,0,1,2,3 };//输出增量kp的隶属值
	float ki_menbership_values[7] = { -3,-2,-1,0,1,2,3 }; //输出增量ki的隶属值
	float kd_menbership_values[7] = { -3,-2,-1,0,1,2,3 };  //输出增量kd的隶属值
	float fuzzyoutput_menbership_values[7] = { -3,-2,-1,0,1,2,3 };

	//int menbership_values[7] = {-3,-};
	float kp;                       //PID参数kp
	float ki;                       //PID参数ki
	float kd;                       //PID参数kd
	float qdetail_kp;               //增量kp对应论域中的值
	float qdetail_ki;               //增量ki对应论域中的值
	float qdetail_kd;               //增量kd对应论域中的值
	float qfuzzy_output;  
	float detail_kp;                //输出增量kp
	float detail_ki;                //输出增量ki
	float detail_kd;                //输出增量kd
	float fuzzy_output;
	float qerro;                    //输入e对应论域中的值
	float qerro_c;                  //输入de/dt对应论域中的值
	float errosum;                  
	float e_gradmembership[2];      //输入e的隶属度
	float ec_gradmembership[2];     //输入de/dt的隶属度
	int e_grad_index[2];            //输入e隶属度在规则表的索引
	int ec_grad_index[2];           //输入de/dt隶属度在规则表的索引
	float gradSums[7] = {0,0,0,0,0,0,0};
	float KpgradSums[7] = { 0,0,0,0,0,0,0 };   //输出增量kp总的隶属度
	float KigradSums[7] = { 0,0,0,0,0,0,0 };   //输出增量ki总的隶属度
	float KdgradSums[7] = { 0,0,0,0,0,0,0 };   //输出增量kd总的隶属度
	int NB = -3, NM = -2, NS = -1, ZO = 0, PS = 1, PM = 2, PB = 3; //论域隶属值

	int  Kp_rule_list[7][7] = { {PB,PB,PM,PM,PS,ZO,ZO},     //kp规则表
								{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  Ki_rule_list[7][7] = { {NB,NB,NM,NM,NS,ZO,ZO},     //ki规则表
								{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  Kd_rule_list[7][7] = { {PS,NS,NB,NB,NB,NM,PS},    //kd规则表
								{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} };

	int  Fuzzy_rule_list[7][7] = { {PB,PB,PB,PB,PM,ZO,ZO},  
								   {PB,PB,PB,PM,PM,ZO,ZO},
								   {PB,PM,PM,PS,ZO,NS,NM},
								   {PM,PM,PS,ZO,NS,NM,NM},
								   {PS,PS,ZO,NM,NM,NM,NB},
								   {ZO,ZO,ZO,NM,NB,NB,NB},
								   {ZO,NS,NB,NB,NB,NB,NB}};


//private:

};
#endif

FuzzyPID.cpp文件

#include "FuzzyPID.h"
FuzzyPID::FuzzyPID()  //构造函数
{
	kp = 0;
	ki = 0;
	kd = 0;
	fuzzy_output = 0;
	qdetail_kp = 0;
	qdetail_ki = 0;
	qdetail_kd = 0;
	qfuzzy_output = 0;
	errosum = 0;
}

FuzzyPID::~FuzzyPID()//析构函数
{
}

//输入e与de/dt隶属度计算函数///
void FuzzyPID::Get_grad_membership(float erro,float erro_c)   
{
	if (erro > e_membership_values[0] && erro < e_membership_values[6])
	{
		for (int i = 0; i < num_area - 2; i++)
		{
			if (erro >= e_membership_values[i] && erro <= e_membership_values[i + 1])
			{
				e_gradmembership[0] = -(erro - e_membership_values[i + 1]) / (e_membership_values[i + 1] - e_membership_values[i]);
				e_gradmembership[1] = 1+(erro - e_membership_values[i + 1]) / (e_membership_values[i + 1] - e_membership_values[i]);
				e_grad_index[0] = i;
				e_grad_index[1] = i + 1;
				break;
			}
		}
	}
	else
	{
		if (erro <= e_membership_values[0])
		{
			e_gradmembership[0] = 1;
			e_gradmembership[1] = 0;
			e_grad_index[0] = 0;
			e_grad_index[1] = -1;
		}
		else if (erro >= e_membership_values[6])
		{
			e_gradmembership[0] = 1;
			e_gradmembership[1] = 0;
			e_grad_index[0] = 6;
			e_grad_index[1] = -1;
		}
	}

	if (erro_c > ec_membership_values[0] && erro_c < ec_membership_values[6])
	{
		for (int i = 0; i < num_area - 2; i++)
		{
			if (erro_c >= ec_membership_values[i] && erro_c <= ec_membership_values[i + 1])
			{
				ec_gradmembership[0] = -(erro_c - ec_membership_values[i + 1]) / (ec_membership_values[i + 1] - ec_membership_values[i]);
				ec_gradmembership[1] = 1 + (erro_c - ec_membership_values[i + 1]) / (ec_membership_values[i + 1] - ec_membership_values[i]);
				ec_grad_index[0] = i;
				ec_grad_index[1] = i + 1;
				break;
			}
		}
	}
	else
	{
		if (erro_c <= ec_membership_values[0])
		{
			ec_gradmembership[0] = 1;
			ec_gradmembership[1] = 0;
			ec_grad_index[0] = 0;
			ec_grad_index[1] = -1;
		}
		else if (erro_c >= ec_membership_values[6])
		{
			ec_gradmembership[0] = 1;
			ec_gradmembership[1] = 0;
			ec_grad_index[0] = 6;
			ec_grad_index[1] = -1;
		}
	}

}

/获取输出增量kp,ki,kd的总隶属度/
void FuzzyPID::GetSumGrad()
{
	for (int i = 0; i <= num_area - 1; i++)
	{
		KpgradSums[i] = 0;
		KigradSums[i] = 0;
    KdgradSums[i] = 0;

	}
  for (int i=0;i<2;i++)
  {
	  if (e_grad_index[i] == -1)
	  {
	   continue;
	  }
	  for (int j = 0; j < 2; j++)
	  {
		  if (ec_grad_index[j] != -1)
		  {
			  int indexKp = Kp_rule_list[e_grad_index[i]][ec_grad_index[j]] + 3;
			  int indexKi = Ki_rule_list[e_grad_index[i]][ec_grad_index[j]] + 3;
			  int indexKd = Kd_rule_list[e_grad_index[i]][ec_grad_index[j]] + 3;
			  //gradSums[index] = gradSums[index] + (e_gradmembership[i] * ec_gradmembership[j])* Kp_rule_list[e_grad_index[i]][ec_grad_index[j]];
			  KpgradSums[indexKp]= KpgradSums[indexKp] + (e_gradmembership[i] * ec_gradmembership[j]);
			  KigradSums[indexKi] = KigradSums[indexKi] + (e_gradmembership[i] * ec_gradmembership[j]);
			  KdgradSums[indexKd] = KdgradSums[indexKd] + (e_gradmembership[i] * ec_gradmembership[j]);
		  }
		  else
		  {
		    continue;
		  }

	  }
  }

}

计算输出增量kp,kd,ki对应论域值//
void FuzzyPID::GetOUT()
{
	for (int i = 0; i < num_area - 1; i++)
	{
		qdetail_kp += kp_menbership_values[i] * KpgradSums[i];
		qdetail_ki += ki_menbership_values[i] * KigradSums[i];
		qdetail_kd+= kd_menbership_values[i] * KdgradSums[i];
	}
}

//模糊PID控制实现函数/
float FuzzyPID::FuzzyPIDcontroller(float e_max, float e_min, float ec_max, float ec_min, float kp_max, float kp_min, float erro, float erro_c,float ki_max,float ki_min,float kd_max,float kd_min,float erro_pre,float errp_ppre)
{
	errosum += erro;
	//Arear_dipart(e_max, e_min, ec_max, ec_min, kp_max, kp_min,ki_max,ki_min,kd_max,kd_min);
	qerro = Quantization(e_max, e_min, erro);
	qerro_c = Quantization(ec_max, ec_min, erro_c);
	Get_grad_membership(qerro, qerro_c);
	GetSumGrad();
	GetOUT();
	detail_kp = Inverse_quantization(kp_max, kp_min, qdetail_kp);
	detail_ki = Inverse_quantization(ki_max, ki_min, qdetail_ki);
	detail_kd = Inverse_quantization(kd_max, kd_min, qdetail_kd);
	qdetail_kd = 0;
	qdetail_ki = 0;
	qdetail_kp = 0;
	/*if (qdetail_kp >= kp_max)
		qdetail_kp = kp_max;
	else if (qdetail_kp <= kp_min)
		qdetail_kp = kp_min;
	if (qdetail_ki >= ki_max)
		qdetail_ki = ki_max;
	else if (qdetail_ki <= ki_min)
		qdetail_ki = ki_min;
	if (qdetail_kd >= kd_max)
		qdetail_kd = kd_max;
	else if (qdetail_kd <= kd_min)
		qdetail_kd = kd_min;*/
	kp = kp + detail_kp;
	ki = ki + detail_ki;
	kd = kd + detail_kd;
	if (kp < 0)
		kp = 0;
	if (ki < 0)
		ki = 0;
	if (kd < 0)
		kd = 0;
	detail_kp = 0;
  detail_ki=0;
  detail_kd=0;
  float output = kp*(erro - erro_pre) + ki * erro + kd * (erro - 2 * erro_pre + errp_ppre);
	return output;
}

///区间映射函数///
float FuzzyPID::Quantization(float maximum,float minimum,float x)
{
	float qvalues= 6.0 *(x-minimum)/(maximum - minimum)-3;
	//float qvalues=6.0*()
	return qvalues;
	
	//qvalues[1] = 3.0 * ecerro / (maximum - minimum);
}

//反区间映射函数
float FuzzyPID::Inverse_quantization(float maximum, float minimum, float qvalues)
{
	float x = (maximum - minimum) *(qvalues + 3)/6 + minimum;
	return x;
}

测试模糊PID控制器：

#include <iostream>
#include "FuzzyPID.h"

int main()
{
    FuzzyPID myfuzzypid;
    float Target = 600;
    float actual = 0;
    float e_max =1000;
    float e_min = -1000;
    float ec_max = 800;
    float ec_min = -800;
    float kp_max =100;
    float kp_min = -100;
    float ki_max = 0.1;
    float ki_min = -0.1;
    float kd_max = 0.01;
    float kd_min = -0.01;
    float erro;
    float erro_c;
    float erro_pre = 0;
    float erro_ppre = 0;
    erro =Target - actual;
    erro_c = erro - erro_pre;
    for (int i = 0; i < 100; i++)
    {
        float u;
        u = myfuzzypid.FuzzyPIDcontroller(e_max, e_min, ec_max, ec_min, kp_max, kp_min, erro, erro_c,ki_max,ki_min,kd_max,kd_min,erro_pre,erro_ppre);
        actual +=u;
        erro_ppre = erro_pre;
        erro_pre = erro;
        erro = Target - actual;
        erro_c= erro - erro_pre;
        std::cout << "i:" << i << "\t" << "Target:" << Target << "\t" << "Actual:" << actual  << std::endl;
    }
 }

运行结果：

三. Arduino 智能小车直线模糊PID算法

主要硬件：Arduino Nano，MPU6050，TT马达，L298N电机驱动模块。

模糊PID控制的目标，输入与输出：

输入：MPU6050测量小车的转动的角度，角速度。

输出：控制TT马达的PWM

目标：小车转动角度为0

进行无模糊PID控制与有模糊PID控制直线行走的效果

无模糊PID控制，角度时间图:

有上图看到，当相同PWM作用与两个TT马达时，由于两个马达的转速不一样，也由于车子装配精度不够，导致车子前进时一直往一边偏，即角度不断增大。

有模糊PID控制效果图:

由与无模糊PID控制器比较，由比较明显的效果，小车基本是在[-10,10]度之间飘动。

四，结语

模糊PID比传统的PID算法的省去了参数调节的麻烦，对于对PID参数调节没经验的人模糊PID算法还是比较容易，但是模糊PID算法控制精度可能比较低。

qq_33276575

关注

25
点赞
踩
237

收藏

觉得还不错? 一键收藏
2
评论
Arduino智能小车直线控制-模糊PID控制

本文主要是基于Arduino智能小车直线走不直的问题，运用模糊PID算法进行控制。本文主要由三部分构成：模糊PID控制器的原理，模糊PID控制器Ｃ＋＋的实现与测试，把模糊PID控制算法应用于Arduino 智能小车走直线的过程中。一，模糊PID原理模糊PID控制流程如下图所示，把目标值Xtarget与输出值Xout的误差e与e的变化率de/dt作为模...
复制链接

扫一扫