陀螺儀模糊控制2維三角形算法程序源碼

ID:302522 · 發(fā)表于 2018-4-5 20:31

互相交流

模糊控制2維三角形算法如下:

#include "Fuzzy.h"
//二維模糊控制系統(tǒng)
//輸入方向偏差與陀螺儀Y軸數(shù)據(jù)，控制輸出方向P
float Angle_E[3] = {5,10,15}; //角度偏差
float Gyro_E[3] = {10,15,30}; //角速度數(shù)據(jù)
float Shell_P[3] = {10,15,40}; //外層比例控制系數(shù)
//由于所測數(shù)據(jù)均為單向數(shù)據(jù)，故規(guī)則表擬定為單向
int rule[3][3]=
{
{2,1,0},
{1,1,0},
{0,0,0},
};
float Fuzzy_Shell_P(float E_angle,float E_gyro);
float Fuzzy_Shell_P(float E_angle,float E_gyro)
{
float angle_err=0,gyro_err=0;
int angle_N=0,gyro_N=0; //隸屬語言值
int num=0;
float fuzzy_P=0;
//記錄隸屬函數(shù)的輸出值
float Angle_Fzy[2] ={0.0,0.0};
float Gyro_Fzy[2] ={0.0,0.0};
float Shell_P_Fzy[3] ={0.0,0.0,0.0};
// /*----- 數(shù)據(jù)更新-----*/
// Shell_P[1] = P_Direction_low;
// Shell_P[2] = P_Direction_high;
//將數(shù)據(jù)取絕對值，數(shù)據(jù)的正負僅代表左右
ABS_val(E_angle,angle_err);
ABS_val(E_gyro,gyro_err);
/*----- Angle 隸屬函數(shù)描述-----*/
if(angle_err<Angle_E[0]) //1
{
Angle_Fzy[0] =1.0;
angle_N = 0;
}
else if(angle_err<Angle_E[1]) //2
{
Angle_Fzy[0] = (Angle_E[1]-angle_err)/(Angle_E[1]-Angle_E[0]);
angle_N = 0;
}
else if(angle_err<Angle_E[2]) //3
{
Angle_Fzy[0] = (Angle_E[2]-angle_err)/(Angle_E[2]-Angle_E[1]);
angle_N = 1;
}
else
{
Angle_Fzy[0] =0;//4
angle_N =1;
}
Angle_Fzy[1] = 1.0 - Angle_Fzy[0];//隸屬函數(shù)輸出值
/*----- Pre_dirr 變化隸屬函數(shù)描述-----*/
if(gyro_err<Gyro_E[0])//當陀螺儀Y軸數(shù)據(jù)小于40時，對應三角隸屬函數(shù)的隸屬度為1，方向量度模糊等級為0級
{
Gyro_Fzy[0] =1;
gyro_N = 0;
}
else if(gyro_err<Gyro_E[1])//當陀螺儀Y軸數(shù)據(jù)小于70時，對應三角隸屬函數(shù)的隸屬度根據(jù)公式得出，方向量度模糊等級為0級，此時為40三角形的右邊一半
{
Gyro_Fzy[0] = (Gyro_E[1] - gyro_err)/(Gyro_E[1]-Gyro_E[0]);
gyro_N = 0 ;
}
else if(gyro_err<Gyro_E[2])//當陀螺儀Y軸數(shù)據(jù)小于160時，對應三角隸屬函數(shù)的隸屬度根據(jù)公式得出，方向量度模糊等級為1級，此時為70三角形的左邊一半
{
Gyro_Fzy[0] = (Gyro_E[2] - gyro_err)/(Gyro_E[2]-Gyro_E[1]);
gyro_N = 1;
}
else //當陀螺儀Y軸數(shù)據(jù)大于160時，對應三角隸屬函數(shù)的隸屬度根據(jù)公式得出，方向量度模糊等級為1級，此時為70三角形的右邊一半
{
Gyro_Fzy[0] =0.0;
gyro_N = 1;
}
//一個數(shù)據(jù)在三角形隸屬度函數(shù)中對應著兩個相鄰三角形中的隸屬度，兩個隸屬度和為1
Gyro_Fzy[1] = 1.0 - Gyro_Fzy[0];
/*查詢模糊規(guī)則表*/
//查表的時候不單單查詢一個規(guī)則，而是向右向下拓展成一個規(guī)則正方形查詢
num =rule[gyro_N][angle_N];
Shell_P_Fzy[num] += Gyro_Fzy[0]*Angle_Fzy[0];
num =rule[gyro_N+1][angle_N];
Shell_P_Fzy[num] += Gyro_Fzy[1]*Angle_Fzy[0];
num =rule[gyro_N][angle_N+1];
Shell_P_Fzy[num] += Gyro_Fzy[0]*Angle_Fzy[1];
num =rule[gyro_N+1][angle_N+1];
Shell_P_Fzy[num] += Gyro_Fzy[1]*Angle_Fzy[1];
/*面積中心法解模糊*/
fuzzy_P=Shell_P_Fzy[0]*Shell_P[0]+Shell_P_Fzy[1]*Shell_P[1]+Shell_P_Fzy[2]*Shell_P[2];
return(fuzzy_P);
}

復制代碼

所有資料51hei提供下載:

Fuzzy.zip (1.27 KB, 下載次數(shù): 19)

帳號		自動登錄	找回密碼
密碼			立即注冊