目录
模糊PID代码使用方法
首先放模糊PID的C语言程序已经使用方法。
这是模糊KP的程序，输入的参数为E（误差err）和EC（err的插值，及微分，err-last_err），返回的参数为动态的KP，然后带入普通的PID计算程序使用
//使用的模糊PID程序，有几个特殊步骤，输入的参数E为err，EC为err的微分，即这次的err减去上次的err
float KP_Fuzzy(float E,float EC)
{
int rule_p[7][7]=
{
{ 6 , 5 , 4 , 4 , 3 , 0 , 0},//-36
{ 6 , 4 , 3 , 3 , 2 , 0 , 0},//-24
{ 4 , 3 , 2 , 1 , 0 , 1 , 2},//-12
{ 2 , 1 , 1 , 0 , 1 , 1 , 2},//0
{ 2 , 1 , 0 , 1 , 2 , 3 , 4},//12
{ 0 , 0 , 2 , 3 , 3 , 4 , 6},//24
{ 0 , 1 , 3 , 4 , 4 , 5 , 6},//36
};//模糊规则表 P
uint8 i2;
/*输入量P语言值特征点*/
float EFF[7]={-30,-20,-10,0,10,20,30};
/*输入量D语言值特征点*/
float DFF[7]={-15,-10,-5,0,5,10,15};
/*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
float UFF[7];
for(i2=0;i2<7;i2++)
UFF[i2]=kp_m/6*i2;
float U=0; /*偏差,偏差微分以及输出值的精确量*/
float PF[2]={0},DF[2]={0},UF[4]={0};
/*偏差,偏差微分以及输出值的隶属度*/
int Pn=0,Dn=0,Un[4]={0};
float t1=0,t2=0,t3=0,t4=0,temp1=0,temp2=0;
/*隶属度的确定*/
/*根据PD的指定语言值获得有效隶属度*/
if(E>EFF[0] && E<EFF[6])
{
if(E<=EFF[1])
{
Pn=-2;
PF[0]=(EFF[1]-E)/(EFF[1]-EFF[0]);
}
else if(E<=EFF[2])
{
Pn=-1;
PF[0]=(EFF[2]-E)/(EFF[2]-EFF[1]);
}
else if(E<=EFF[3])
{
Pn=0;
PF[0]=(EFF[3]-E)/(EFF[3]-EFF[2]);
}
else if(E<=EFF[4])
{
Pn=1;
PF[0]=(EFF[4]-E)/(EFF[4]-EFF[3]);
}
else if(E<=EFF[5])
{
Pn=2;
PF[0]=(EFF[5]-E)/(EFF[5]-EFF[4]);
}
else if(E<=EFF[6])
{
Pn=3;
PF[0]=(EFF[6]-E)/(EFF[6]-EFF[5]);
}
}
else if(E<=EFF[0])
{
Pn=-2;/* ??? */
PF[0]=1;
}
else if(E>=EFF[6])
{
Pn=3;
PF[0]=0;
}
PF[1]=1-PF[0];
//判断D的隶属度
if(EC>DFF[0]&&EC<DFF[6])
{
if(EC<=DFF[1])
{
Dn=-2;
DF[0]=(DFF[1]-EC)/(DFF[1]-DFF[0]);
}
else if(EC<=DFF[2])
{
Dn=-1;
DF[0]=(DFF[2]-EC)/(DFF[2]-DFF[1]);
}
else if(EC<=DFF[3])
{
Dn=0;
DF[0]=(DFF[3]-EC)/(DFF[3]-DFF[2]);
}
else if(EC<=DFF[4])
{
Dn=1;
DF[0]=(DFF[4]-EC)/(DFF[4]-DFF[3]);
}
else if(EC<=DFF[5])
{
Dn=2;
DF[0]=(DFF[5]-EC)/(DFF[5]-DFF[4]);
}
else if(EC<=DFF[6])
{
Dn=3;
DF[0]=(DFF[6]-EC)/(DFF[6]-DFF[5]);
}
}
//不在给定的区间内
else if (EC<=DFF[0])
{
Dn=-2;
DF[0]=1;
}
else if(EC>=DFF[6])
{
Dn=3;
DF[0]=0;
}
DF[1]=1-DF[0];
/*使用误差范围优化后的规则表rule[7][7]*/
/*输出值使用13个隶属函数,中心值由UFF[7]指定*/
/*一般都是四个规则有效*/
Un[0]=rule_p[Pn+2][Dn+2];
Un[1]=rule_p[Pn+3][Dn+2];
Un[2]=rule_p[Pn+2][Dn+3];
Un[3]=rule_p[Pn+3][Dn+3];
if(PF[0]<=DF[0]) //求小
UF[0]=PF[0];
else
UF[0]=DF[0];
if(PF[1]<=DF[0])
UF[1]=PF[1];
else
UF[1]=DF[0];
if(PF[0]<=DF[1])
UF[2]=PF[0];
else
UF[2]=DF[1];
if(PF[1]<=DF[1])
UF[3]=PF[1];
else
UF[3]=DF[1];
/*同隶属函数输出语言值求大*/
if(Un[0]==Un[1])
{
if(UF[0]>UF[1])
UF[1]=0;
else
UF[0]=0;
}
if(Un[0]==Un[2])
{
if(UF[0]>UF[2])
UF[2]=0;
else
UF[0]=0;
}
if(Un[0]==Un[3])
{
if(UF[0]>UF[3])
UF[3]=0;
else
UF[0]=0;
}
if(Un[1]==Un[2])
{
if(UF[1]>UF[2])
UF[2]=0;
else
UF[1]=0;
}
if(Un[1]==Un[3])
{
if(UF[1]>UF[3])
UF[3]=0;
else
UF[1]=0;
}
if(Un[2]==Un[3])
{
if(UF[2]>UF[3])
UF[3]=0;
else
UF[2]=0;
}
t1=UF[0]*UFF[Un[0]];
t2=UF[1]*UFF[Un[1]];
t3=UF[2]*UFF[Un[2]];
t4=UF[3]*UFF[Un[3]];
temp1=t1+t2+t3+t4;
temp2=UF[0]+UF[1]+UF[2]+UF[3];//模糊量输出
if(temp2!=0)
U=temp1/temp2;
else {
U=0;
}
// temp1=PF[0]*UFF[Un[0]]+PF[1]*UFF[Un[1]]+PF[0]*UFF[Un[2]]+PF[1]*UFF[Un[3]]+DF[0]*UFF[Un[0]]+DF[0]*UFF[Un[1]]+DF[1]*UFF[Un[2]]+DF[0]*UFF[Un[3]];
// U=temp1;
return U;
}
下面是模糊KD的程序，使用的为一阶的，输入的参数为EC（err-last_err），返回的为动态的KD
int rule_d[7] = { 6 , 5 , 3 , 2 , 3 , 5 , 6};//模糊规则表 D
float Kd_Fuzzy(float EC)
{
float out=0;
uint8 i=0;
float degree_left = 0,degree_right = 0;
uint8 degree_left_index = 0,degree_right_index = 0;
float DFF[7]={-15,-10,-5,0,5,10,15};
float UFF[7];
for(i=0;i<7;i++)
UFF[i]=kd_m/6*i;
if(EC<DFF[0])
{
degree_left = 1;
degree_right = 0;
degree_left_index = 0;
}
else if (EC>DFF[6]) {
degree_left = 1;
degree_right = 0;
degree_left_index = 6;
}
else {
for(i=0;i<6;i++)
{
if(EC>=DFF[i]&&EC<DFF[i+1])
{
degree_left = (float)(DFF[i+1] - EC)/(DFF[i+1] - DFF[i]);
degree_right = 1 - degree_left;
degree_left_index = i;
degree_right_index = i+1;
break;
}
}
}
out = UFF[rule_d[degree_left_index]]*degree_left+UFF[rule_d[degree_right_index]]*degree_right;
return out;
}
然后带入普通的PID函数进行控制
//舵机pid控制函数
void pid_servo(void)
{
float E=0,EC=0;
E = err;
EC = err - err_last;
float Kp,Kd,output;
//kp_m为动态的Kp最大值，kd_m为动态的最大Kd值
kp_m = 11.8 ;
kd_m = 13.0 ;
Kp = KP_Fuzzy(E,EC);
Kd = Kd_Fuzzy(EC) ;
output = -( Kp*E+Kd*(EC) );
servo_control(output);
}参数调整方法
模糊Kp的参数调整一共有三个部分，kp_m，EFF，UFF。
第一个部分为EFF，即err的范围，不细究原理的话，可以理解为，EFF为E（即err的范围），DFF为EC（ | err-last_err| 的范围），如下，当err大于30，（err-last_err）大于15时，动态输出的Kp即为kp_m，Kp输出的最大值。
第二个部分为UFF，即（err-last_err）的范围，理解同上
调整时，可以把车子放在赛道的大弯道的地方，找到大概的err，把大弯道的err最为最外围（如下列代码中的30），中等弯道的err作为中间（如下列代码中的20），小弯道作为内围（如下列代码中的10），填充EFF数组的值。然后对于UFF，可以设置为EFF的一半或三分之一。
第三个部分为kp_m，及为kp的最大值，实际上是将kp_m线性分割为6个部分，即kp_m除以7分为6段，[0,kp_m/7]，[kp_m/7,2*kp_m/7]......[6*kp_m/7,kp_m]，调整参数的时候，简单调整方法就是，设置好上面的EFF[7]，DFF[7]数组后，调kp_m，可以把kp_m当作普通kp来调。
模糊kd的参数调整和模糊kp一样，只是为一维。注意要把kp_m和kd_m设置为全局变量。
模糊PID的原理详解
对于模糊PID的详细原理和计算过程，我放在我的另一篇文章中，链接如下：
扫一扫
1614
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
