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

本文主要是基于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文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196

#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控制器：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

#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算法控制精度可能比较低。