智能车竞赛
硬件方面
人机交互
环岛处理
方向控制
速度闭环
偏差计算
电磁数据处理
本文档使用 MrDoc 发布
-
+
home page
方向控制
# 方向控制 ## 方向控制 ```C //增量式PID完整示例 SpeedError[2][2]=deviation; SpeedPID = Kp3 * (SpeedError[2][2] - SpeedError[2][1]) + Ki3 * (SpeedError[2][2]) + Kd3 * (SpeedError[2][2] - 2*SpeedError[2][1] + SpeedError[2][0]); //位置式PID完整示例 float Err_Sum;//用于记录累计积分 Err_Sum += SpeedError[2][2]; DirectionPID = Kp3 * SpeedError[2][2] + Ki3 * Err_Sum + Kd3 * (SpeedError[2][2] - SpeedError[2][1]); ``` ### 位置PD 由于控制对象，小车本身有一定的质量（惯性），所以无需I控制，参考上述完整示例可发现此时增量位置PID已无区别。 ```C float DirectionPID; #define Kp3 3.3 //比例常数 #define Kd3 0.3 //微分常数 int PID_Direction(int deviation) { SpeedError[2][2]=deviation; DirectionPID = Kp3 * SpeedError[2][2] + Kd3 * (SpeedError[2][2] - SpeedError[2][1]); for(uint8 i=0;i<2;i++) { SpeedError[2][i]=SpeedError[2][i+1];//数据左移，低位扔掉 } return DirectionPID; } ``` 最后输出的是偏差量，不能直接传给速度环，需要加上基础速度。 ### 模糊PD ```C /*******!!! 该宏定义不可修改 !!!******/ #define NB 1 #define NM 2 #define NS 3 #define ZO 4 #define PS 5 #define PM 6 #define PB 7 /*** 不可修改内容结束 ****/ //N为negative，P为positive，B为big，M为middle，S为small，ZO为zero int8 DirectionError[2][3]={0};//-128~127，这里储存偏差量，注意数字范围 #define E_MAX 120 //偏差E的最大值（预设），程序会自动分割成正负对称数列 #define EC_MAX 60 //偏差变化EC的最大值（预设），程序会自动分割成正负对称数列 #define P_MAX 10 //方向环P参数的最大值（预设），若不需要负数列，请修改下面的自动分割逻辑 #define D_MAX 10 //方向环D参数的最大值（预设），若不需要负数列，请修改下面的自动分割逻辑 int8 E_NB,E_NM,E_NS,E_ZO,E_PS,E_PM,E_PB; //E的界限值 float E_NB_D,E_NM_D,E_NS_D,E_ZO_D,E_PS_D,E_PM_D,E_PB_D; //用于保存E的隶属度 int8 EC_NB,EC_NM,EC_NS,EC_ZO,EC_PS,EC_PM,EC_PB; //EC的界限值 float EC_NB_D,EC_NM_D,EC_NS_D,EC_ZO_D,EC_PS_D,EC_PM_D,EC_PB_D;//用于保存EC的隶属度 float E_EC_NB_D,E_EC_NM_D,E_EC_NS_D,E_EC_ZO_D,E_EC_PS_D,E_EC_PM_D,E_EC_PB_D;//用于保存E_EC的隶属度 float P_NB,P_NM,P_NS,P_ZO,P_PS,P_PM,P_PB; //输出P的界限值 float D_NB,D_NM,D_NS,D_ZO,D_PS,D_PM,D_PB; //输出D的界限值 float P_out,D_out; //用于保存最后的P，D输出 int16 Direction_PID; //用于保存方向PID的输出（直接加在速度环的输入上，此时速度环必需指定基准速度） float Fuzzy_a,Fuzzy_b,Fuzzy_c,Fuzzy_d; void Fuzzy_Preprocess(void)//用于自动处理预设数据 { //自动等分，只需运行一次 Fuzzy_a=E_MAX/4; Fuzzy_b=EC_MAX/4; Fuzzy_c=P_MAX/4; Fuzzy_d=D_MAX/4; E_NB = -E_MAX+Fuzzy_a,E_NM = -E_MAX+Fuzzy_a*2,E_NS = -E_MAX+Fuzzy_a*3,E_ZO=E_MAX-Fuzzy_a*4,E_PS = E_MAX-Fuzzy_a*3,E_PM = E_MAX-Fuzzy_a*2,E_PB = E_MAX-Fuzzy_a; EC_NB = -EC_MAX+Fuzzy_b,EC_NM = -EC_MAX+Fuzzy_b*2,EC_NS = -EC_MAX+Fuzzy_b*3,EC_ZO=EC_MAX-Fuzzy_b*4,EC_PS = EC_MAX-Fuzzy_b*3,EC_PM = EC_MAX-Fuzzy_b*2,EC_PB = EC_MAX-Fuzzy_b; P_NB = -P_MAX+Fuzzy_c,P_NM = -P_MAX+Fuzzy_c*2,P_NS = -P_MAX+Fuzzy_c*3,P_ZO=P_MAX-Fuzzy_c*4,P_PS = P_MAX-Fuzzy_c*3,P_PM = P_MAX-Fuzzy_c*2,P_PB = P_MAX-Fuzzy_c; D_NB = -D_MAX+Fuzzy_d,D_NM = -D_MAX+Fuzzy_d*2,D_NS = -D_MAX+Fuzzy_d*3,D_ZO=D_MAX-Fuzzy_d*4,D_PS = D_MAX-Fuzzy_d*3,D_PM = D_MAX-Fuzzy_d*2,D_PB = D_MAX-Fuzzy_d; //用于保存预处理好的范围数据 int8 E_scope[9]={-E_MAX,E_NB,E_NM,E_NS,E_ZO,E_PS,E_PM,E_PB,E_MAX};//误差范围 int8 EC_scope[9]={-EC_MAX,EC_NB,EC_NM,EC_NS,EC_ZO,EC_PS,EC_PM,EC_PB,EC_MAX};//误差变化范围 int8 P_scope[9]={-P_MAX,P_NB,P_NM,P_NS,P_ZO,P_PS,P_PM,P_PB,P_MAX};//P范围 int8 D_scope[9]={-D_MAX,D_NB,D_NM,D_NS,D_ZO,D_PS,D_PM,D_PB,D_MAX};//D范围 } //用于保存模糊规则 EC NB NM NS ZO PS PM PB int8 Fuzzy_Rule[7*4][7]={{PB,PB,PB,PB,PB,PB,PB},//模糊控制表 {PB,PB,PB,PB,PM,PM,PS}, {PM,PM,PM,PS,PS,ZO,ZO},//该表用于直接控制，即输出为实际物理量 {PM,PS,PS,ZO,NS,NM,NM},//该表可根据需要修改 {ZO,NS,NS,NM,NM,NM,NB}, {NS,NS,NM,NM,NM,NB,NB}, {NM,NM,NB,NB,NB,NB,NB}, /*E*/ /*NB*/{PB,PB,PM,PM,PS,ZO,ZO},//模糊P规则表 /*NM*/{PB,PB,PM,PS,PS,ZO,NS}, /*NS*/{PM,PM,PM,PS,ZO,NS,NS},//该表用于控制PID的参数P，即输出为P值或P值的变化量 /*ZO*/{PM,PM,PS,ZO,NS,NM,NM},//该表必须修改，不可直接使用 /*PS*/{PS,PS,ZO,NS,NS,NM,NM}, /*PM*/{PS,ZO,NS,NM,NM,NM,NB}, /*PB*/{ZO,ZO,NM,NM,NM,NB,NB}, {NB,NB,NM,NM,NS,ZO,ZO},//模糊I规则表 {NB,NB,NM,NS,NS,ZO,ZO}, {NB,NM,NS,NS,ZO,PS,PS},//该表用于控制PID的参数I，即输出为I值或I值的变化量 {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}, {PS,NS,NB,NB,NB,NM,PS},//模糊D规则表 {PS,NS,NB,NM,NM,NS,ZO}, {ZO,NS,NM,NM,NS,NS,ZO},//该表用于控制PID的参数D，即输出为D值或D值的变化量 {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}, }; //用于指引模糊判断，临时变量 int8 e_cache=0; int8 ec_cache=0; void Fuzzy_Menbership(int E)//计算输入隶属度 { //记录偏差 int EC; EC=E-DirectionError[0][2]; for(uint8 i=0;i<2;i++) { DirectionError[0][i]=DirectionError[0][i+1];//数据左移，低位扔掉 } DirectionError[0][2]=E; //计算模糊输入隶属度 if(E <= E_NB) E_NB_D=1,e_cache=0; if(E > E_NB && E < E_NM) E_NB_D=(E_NM-E)/Fuzzy_a,E_NM_D=(E-E_NB)/Fuzzy_a,e_cache=1; if(E > E_NM && E < E_NS) E_NM_D=(E_NS-E)/Fuzzy_a,E_NS_D=(E-E_NM)/Fuzzy_a,e_cache=2; if(E > E_NS && E < E_ZO) E_NS_D=(E_ZO-E)/Fuzzy_a,E_ZO_D=(E-E_NS)/Fuzzy_a,e_cache=3; if(E > E_ZO && E < E_PS) E_ZO_D=(E_PS-E)/Fuzzy_a,E_PS_D=(E-E_ZO)/Fuzzy_a,e_cache=4; if(E > E_PS && E < E_PM) E_PS_D=(E_PM-E)/Fuzzy_a,E_PM_D=(E-E_PS)/Fuzzy_a,e_cache=5; if(E > E_PM && E < E_PB) E_PM_D=(E_PB-E)/Fuzzy_a,E_PB_D=(E-E_PM)/Fuzzy_a,e_cache=6; if(E >= E_PB) E_PB_D=1,e_cache=7; if(EC <= EC_NB) EC_NB_D=1,ec_cache=0; if(EC > EC_NB && EC < EC_NM) EC_NB_D=(EC_NM-EC)/Fuzzy_b,EC_NM_D=(EC-EC_NB)/Fuzzy_b,ec_cache=1; if(EC > EC_NM && EC < EC_NS) EC_NM_D=(EC_NS-EC)/Fuzzy_b,EC_NS_D=(EC-EC_NM)/Fuzzy_b,ec_cache=2; if(EC > EC_NS && EC < EC_ZO) EC_NS_D=(EC_ZO-EC)/Fuzzy_b,EC_ZO_D=(EC-EC_NS)/Fuzzy_b,ec_cache=3; if(EC > EC_ZO && EC < EC_PS) EC_ZO_D=(EC_PS-EC)/Fuzzy_b,EC_PS_D=(EC-EC_ZO)/Fuzzy_b,ec_cache=4; if(EC > EC_PS && EC < EC_PM) EC_PS_D=(EC_PM-EC)/Fuzzy_b,EC_PM_D=(EC-EC_PS)/Fuzzy_b,ec_cache=5; if(EC > EC_PM && EC < EC_PB) EC_PM_D=(EC_PB-EC)/Fuzzy_b,EC_PB_D=(EC-EC_PM)/Fuzzy_b,ec_cache=6; if(EC >= EC_PB) EC_PB_D=1,ec_cache=7; } //用于保存经过预处理的输出隶属度 float Fuzzy_MP[7*4][8]={0}; //读取规则并写入融合隶属度 void Fuzzy_RD(int a,int b)//a：数据存储模式 b：规则切换 { switch (a) { case 1:switch(Fuzzy_Rule[e_cache+7*b][ec_cache]) { case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache+7*b][ec_cache];break; };break; case 2:switch(Fuzzy_Rule[e_cache+7*b][ec_cache]) { case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache];break; };break; case 3:switch(Fuzzy_Rule[e_cache+7*b][ec_cache]) { case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache+7*b][ec_cache-1];break; };break; case 4:switch(Fuzzy_Rule[e_cache+7*b][ec_cache]) { case 1:E_EC_NB_D=E_EC_NB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; case 2:E_EC_NM_D=E_EC_NM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; case 3:E_EC_NS_D=E_EC_NS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; case 4:E_EC_ZO_D=E_EC_ZO_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; case 5:E_EC_PS_D=E_EC_PS_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; case 6:E_EC_PM_D=E_EC_PM_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; case 7:E_EC_PB_D=E_EC_PB_D+Fuzzy_MP[e_cache-1+7*b][ec_cache-1];break; };break; } } //模糊引擎 float Fuzzy_Engine(int8 a)//0：模糊控制 1：模糊P 2：模糊I 3：模糊D { E_EC_NB_D=E_EC_NM_D=E_EC_NS_D=E_EC_ZO_D=E_EC_PS_D=E_EC_PM_D=E_EC_PB_D=0;//模糊输出隶属度预处理（清零） switch(e_cache) { case 0: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NB_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 1: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NB_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NM_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NB_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 2: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NM_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_NS_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 3: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_NS_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_ZO_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_NS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 4: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_ZO_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PS_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_ZO_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 5: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PS_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PM_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PS_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 6: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_NB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_NB_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_NM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_NM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_NS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_NS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_ZO_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_ZO_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PS_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_PS_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PM_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache-1+7*a][ec_cache-1]= E_PM_D*EC_PM_D,Fuzzy_RD(4,a);Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache-1+7*a][ec_cache]= E_PM_D*EC_PB_D,Fuzzy_RD(2,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; case 7: { switch (ec_cache) { case 0:Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NB_D,Fuzzy_RD(1,a);break; case 1:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NB_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NM_D,Fuzzy_RD(1,a);break; case 2:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_NS_D,Fuzzy_RD(1,a);break; case 3:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_NS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_ZO_D,Fuzzy_RD(1,a);break; case 4:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_ZO_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PS_D,Fuzzy_RD(1,a);break; case 5:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PS_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PM_D,Fuzzy_RD(1,a);break; case 6:Fuzzy_MP[e_cache+7*a][ec_cache-1]= E_PB_D*EC_PM_D,Fuzzy_RD(3,a);Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break; case 7:Fuzzy_MP[e_cache+7*a][ec_cache]= E_PB_D*EC_PB_D,Fuzzy_RD(1,a);break; } }break; } } int Fuzzy_PID(int E)//模糊调用函数示范 { Fuzzy_Preprocess();//用于自动处理预设数据，只需运行一次 Fuzzy_Menbership(E);//计算输入隶属度 Fuzzy_Engine(1);//计算模糊P //重心法解模糊 //计算本次需调节的量，其中PD可直接用，也可加在预设的基准值上，这取决于PD的变化范围 P_out = E_EC_NB_D*P_NB + E_EC_NM_D*P_NM + E_EC_NS_D*P_NS + E_EC_ZO_D*P_ZO + E_EC_PS_D*P_PS + E_EC_PM_D*P_PM + E_EC_PB_D*P_PB; Fuzzy_Engine(3);//计算模糊D D_out = E_EC_NB_D*D_NB + E_EC_NM_D*D_NM + E_EC_NS_D*D_NS + E_EC_ZO_D*D_ZO + E_EC_PS_D*D_PS + E_EC_PM_D*D_PM + E_EC_PB_D*D_PB; Direction_PID = P_out * DirectionError[0][2] + D_out * (DirectionError[0][2] - DirectionError[0][1]); return Direction_PID; } ``` 模糊控制通用引擎，可以自行修改调用逻辑使代码执行更高效。 除预设值外，输入只有偏差，输出取决于所选用的模糊规则表。 ### 动态PD 将普通PD控制与模糊PD控制对比可发现，模糊PID本质上是实现了根据不同的偏差（和偏差变化量）自动修改P，D参数的大小。 一般情况下，较大的偏差需要用较大的P削弱，较大的偏差变化率需要用较大的D来超前修正，那么其实可以直接将偏差量叠加到PD参数上来近似实现类似的效果。 ```C //动态PD控制 float Dynamic_P,Dynamic_D;//动态PD参数 int PID_Direction(int deviation) { SpeedError[2][2]=deviation; DirectionPID = (Dynamic_P*SpeedError[2][2]*SpeedError[2][2] + Kp3) * SpeedError[2][2] + (Dynamic_D*(SpeedError[2][2] - SpeedError[2][1])*(SpeedError[2][2] - SpeedError[2][1]) + Kd3) * (SpeedError[2][2] - SpeedError[2][1]); for(uint8 i=0;i<2;i++) { SpeedError[2][i]=SpeedError[2][i+1];//数据左移，低位扔掉 } return DirectionPID; } ``` 代码中对偏差量做了平方处理，这是为了实现在小偏差的情况下尽量使用静态参数，而在大偏差下使用大的动态参数。该方法相对模糊PID要简单很多，稍微损失一点模糊精度就可大幅降低控制调试难度，是中低端处理器的较佳选择。
boiling
Dec. 25, 2023, 10:53 a.m.
转发文档
Collection documents
Last
Next
手机扫码
Copy link
手机扫一扫转发分享
Copy link
Markdown文件
share
link
type
password
Update password