fuzzy.cpp

路径规划源程序

// Fuzzy.cpp: implementation of the Fuzzy class.
//
//////////////////////////////////////////////////////////////////////
//#include "StdAfx.h"
#include "Fuzzy.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


FuzzyControl::FuzzyControl()
{
	////////////////角度语言变量的中心值//////////////////////////////////////////////////////////
	
	m_aAngleMemberFunCenterValue[0]=0;//0
	m_aAngleMemberFunCenterValue[1]=45;//小
	m_aAngleMemberFunCenterValue[2]=90;//中
	m_aAngleMemberFunCenterValue[3]=160;//大
//////////////危险语言变量隶属函数的中心值////////////////////////////////////////////////////////////

	m_aDangerMemberFunCenterValue[0]=0;//0
	m_aDangerMemberFunCenterValue[1]=0.47;//小
	m_aDangerMemberFunCenterValue[2]=0.65;//中
	m_aDangerMemberFunCenterValue[3]=0.84;//大
////////////////角度语言变量隶属函数的形状参数//////////////////////////////////////////////////////////

	m_aAngleMemberFunctionPar[0]=-14;
	m_aAngleMemberFunctionPar[1]=0.4758;
	m_aAngleMemberFunctionPar[2]=16.99;
	m_aAngleMemberFunctionPar[3]=45;
	m_aAngleMemberFunctionPar[4]=16;
	m_aAngleMemberFunctionPar[5]=100;
	m_aAngleMemberFunctionPar[6]=0.202;
	m_aAngleMemberFunctionPar[7]=134;
/////////////////危险语言变量隶属函数的形状参数/////////////////////////////////////////////////////////
	
	m_aDangerMemberFunctionPar[0]=-91;
	m_aDangerMemberFunctionPar[1]=0.4;
	m_aDangerMemberFunctionPar[2]=0.392;
	m_aDangerMemberFunctionPar[3]=0.422;
	m_aDangerMemberFunctionPar[4]=0.5;
	m_aDangerMemberFunctionPar[5]=0.552;
	m_aDangerMemberFunctionPar[6]=0.524;
	m_aDangerMemberFunctionPar[7]=0.56;
	m_aDangerMemberFunctionPar[8]=0.704;
	m_aDangerMemberFunctionPar[9]=0.751;
	m_aDangerMemberFunctionPar[10]=69.1;
	m_aDangerMemberFunctionPar[11]=0.746;
///////////////////////规则的建立,每个规则对应的结果语言变量是多少///////////////////////////////////////////////////
//规则编号的顺序为MATLAB中由下到上的顺序
	m_anSingleRuleResultLineSpeedCenterValue[0]=3;//Angle ZERO,ND ZERO THEN LV ZERO,ANgleV MIDSHARP
	m_anSingleRuleResultCornerSpeedCenterValue[0]=0;
	
	m_anSingleRuleResultLineSpeedCenterValue[1]=2;
	m_anSingleRuleResultCornerSpeedCenterValue[1]=0;

	m_anSingleRuleResultLineSpeedCenterValue[2]=2;
	m_anSingleRuleResultCornerSpeedCenterValue[2]=0;


	m_anSingleRuleResultLineSpeedCenterValue[3]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[3]=1;
//////////////////////////////////////////////////////////////////////////

	m_anSingleRuleResultLineSpeedCenterValue[4]=3;
	m_anSingleRuleResultCornerSpeedCenterValue[4]=2;

	m_anSingleRuleResultLineSpeedCenterValue[5]=3;
	m_anSingleRuleResultCornerSpeedCenterValue[5]=2;

	m_anSingleRuleResultLineSpeedCenterValue[6]=2;
	m_anSingleRuleResultCornerSpeedCenterValue[6]=2;

	m_anSingleRuleResultLineSpeedCenterValue[7]=0;
	m_anSingleRuleResultCornerSpeedCenterValue[7]=1;
//////////////////////////////////////////////////////////////////////////

	m_anSingleRuleResultLineSpeedCenterValue[8]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[8]=2;
		
	m_anSingleRuleResultLineSpeedCenterValue[9]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[9]=2;

	m_anSingleRuleResultLineSpeedCenterValue[10]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[10]=3;

	m_anSingleRuleResultLineSpeedCenterValue[11]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[11]=3;
//////////////////////////////////////////////////////////////////////////

	m_anSingleRuleResultLineSpeedCenterValue[12]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[12]=3;

	m_anSingleRuleResultLineSpeedCenterValue[13]=1;
	m_anSingleRuleResultCornerSpeedCenterValue[13]=2;

	m_anSingleRuleResultLineSpeedCenterValue[14]=0;
	m_anSingleRuleResultCornerSpeedCenterValue[14]=2;
	
	m_anSingleRuleResultLineSpeedCenterValue[15]=0;
	m_anSingleRuleResultCornerSpeedCenterValue[15]=2; 
	///////////////////////线速度输出函数中心///////////////////////////////////////////////////
	m_aLineSpeedMemberFunCenterValue[0]=0;//
	m_aLineSpeedMemberFunCenterValue[1]=0.3;//0.4
	m_aLineSpeedMemberFunCenterValue[2]=0.64;//0.9
	m_aLineSpeedMemberFunCenterValue[3]=1;//1.4

	////////////////////////角速度输出函数中心//////////////////////////////////////////////////
	m_aCornerSpeedMemberFunCenterValue[0]=0;//
	m_aCornerSpeedMemberFunCenterValue[1]=0.32;//0.52
	m_aCornerSpeedMemberFunCenterValue[2]=0.66;//0.76
	m_aCornerSpeedMemberFunCenterValue[3]=1;//
}

FuzzyControl::~FuzzyControl()
{

}

void FuzzyControl::ProcessFuzzyControl(double TargetAngle, double Danger)
{
	memset(m_aFuzzyRuleActiveDegree,0,sizeof(m_aFuzzyRuleActiveDegree));
	FuzzyInput(TargetAngle,Danger);
	int ActiveRuleNum=0;
	double SumRuleActiveDegree=0,CornerSpeedSumRuleWeightOutPut=0,LineSpeedSumRuleWeightOutPut=0;
	for (int i=0;i<4;i++)
	{
		for (int j=0;j<4;j++)
		{
			m_aFuzzyRuleActiveDegree[i*4+j]=m_aAngleBelongDegree[i]*m_aDangerBelongDegree[j];
			if (m_aFuzzyRuleActiveDegree[i*4+j]>0.0005)
			{
				ActiveRuleNum++;
				SumRuleActiveDegree+=m_aFuzzyRuleActiveDegree[i*4+j];
				LineSpeedSumRuleWeightOutPut+=m_aFuzzyRuleActiveDegree[i*4+j]*m_aLineSpeedMemberFunCenterValue[m_anSingleRuleResultLineSpeedCenterValue[i*4+j]];
				CornerSpeedSumRuleWeightOutPut+=m_aFuzzyRuleActiveDegree[i*4+j]*m_aCornerSpeedMemberFunCenterValue[m_anSingleRuleResultCornerSpeedCenterValue[i*4+j]];
				if (ActiveRuleNum>=4)
				{goto GetedFuzzyOutPut;}
			}
		}	
	}
GetedFuzzyOutPut:
	LineSpeedOutPut=LineSpeedSumRuleWeightOutPut/SumRuleActiveDegree;
	CornerSpeedOutPut=CornerSpeedSumRuleWeightOutPut/SumRuleActiveDegree;
	if (LineSpeedOutPut>1.41||LineSpeedOutPut<0) 
	{
		LineSpeedOutPut=1.4;
//		AfxMessageBox("模糊控制LineSpeedOutPut结果超限");
	}
	if (CornerSpeedOutPut>1.1||CornerSpeedOutPut<0) 
	{
		CornerSpeedOutPut=1;
//		AfxMessageBox("模糊控制CornerSpeedOutPut结果超限");
	}
//	TRACE2("LineSpeedOutPut=%f,CornerSpeedOutPut=%f",LineSpeedOutPut,CornerSpeedOutPut);
	return;
}

void FuzzyControl::FuzzyInput(double AngleInput, double DangerInput)
{
	memset(m_aAngleBelongDegree,0,sizeof(m_aAngleBelongDegree));
	memset(m_aDangerBelongDegree,0,sizeof(m_aDangerBelongDegree));
//////////////////角度模糊化////////////////////////////////////////////////////////
	
	if (AngleInput>=m_aAngleMemberFunCenterValue[3]) 
	{
		m_aAngleBelongDegree[3]=1;
	}
	else if (AngleInput>m_aAngleMemberFunCenterValue[2])//100 
	{
		m_aAngleBelongDegree[3]=1/(1+exp(-m_aAngleMemberFunctionPar[6]*(AngleInput-m_aAngleMemberFunctionPar[7])));
		m_aAngleBelongDegree[2]=exp(-pow((AngleInput - m_aAngleMemberFunctionPar[5]),2)/(2*pow(m_aAngleMemberFunctionPar[4],2)));
	}
	else if (AngleInput>m_aAngleMemberFunCenterValue[1]) 
	{
		m_aAngleBelongDegree[2]=exp(-pow((AngleInput - m_aAngleMemberFunctionPar[5]),2)/(2*pow(m_aAngleMemberFunctionPar[4],2)));
		m_aAngleBelongDegree[1]=exp(-pow((AngleInput - m_aAngleMemberFunctionPar[3]),2)/(2*pow(m_aAngleMemberFunctionPar[2],2)));
	}
	else //大于零的其余部分
	{
		m_aAngleBelongDegree[1]=exp(-pow((AngleInput - m_aAngleMemberFunctionPar[3]),2)/(2*pow(m_aAngleMemberFunctionPar[2],2)));
		m_aAngleBelongDegree[0]=exp(-pow((AngleInput - m_aAngleMemberFunctionPar[1]),2)/(2*pow(m_aAngleMemberFunctionPar[0],2)));
	}
///////////////////危险输入的模糊化///////////////////////////////////////////////////////

	if (DangerInput>=m_aDangerMemberFunCenterValue[3]) //Big
	{
		m_aDangerBelongDegree[3]=1;
	}
	else if (DangerInput>m_aDangerMemberFunCenterValue[2])//100 Big,Mid
	{
		m_aDangerBelongDegree[3]=1/(1+exp(-m_aDangerMemberFunctionPar[10]*(DangerInput-m_aDangerMemberFunctionPar[11])));
		m_aDangerBelongDegree[2]=trapmf(DangerInput,m_aDangerMemberFunctionPar[6],m_aDangerMemberFunctionPar[7],m_aDangerMemberFunctionPar[8],m_aDangerMemberFunctionPar[9]);
	}
	else if (DangerInput>m_aDangerMemberFunCenterValue[1])// Mid,Small
	{
		m_aDangerBelongDegree[2]=trapmf(DangerInput,m_aDangerMemberFunctionPar[6],m_aDangerMemberFunctionPar[7],m_aDangerMemberFunctionPar[8],m_aDangerMemberFunctionPar[9]);
		m_aDangerBelongDegree[1]=trapmf(DangerInput,m_aDangerMemberFunctionPar[2],m_aDangerMemberFunctionPar[3],m_aDangerMemberFunctionPar[4],m_aDangerMemberFunctionPar[5]);
	}
	else //大于零的其余部分
	{
		m_aDangerBelongDegree[1]=trapmf(DangerInput,m_aDangerMemberFunctionPar[2],m_aDangerMemberFunctionPar[3],m_aDangerMemberFunctionPar[4],m_aDangerMemberFunctionPar[5]);
		m_aDangerBelongDegree[0]=1/(1+exp(-m_aDangerMemberFunctionPar[0]*(DangerInput-m_aDangerMemberFunctionPar[1])));
	}
}

//double FuzzyControl::sigmf(double sig1, double c1, double sig2, double c2)
//{
//	return 0;
// }
inline double FuzzyControl::trapmf(double inputx,double sig1, double c1, double sig2, double c2)
{
	if (inputx<sig1||inputx>c2) //sig1,c1,sig2,c2
	{
		return 0;
	}
	else if (inputx<=sig2&&inputx>=c1) 
	{
		return 1;
	}
	else if (inputx>=sig1&&inputx<c1) 
	{
		return (inputx-sig1)/(c1-sig1);
	}
	else// if (inputx>=sig2&&inputx<c1) 
	{
		return 1-(inputx-sig2)/(c2-sig2);
	}
}