📄 greypidcontrol.cpp
字号:
// greyPIDcontrol.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include "greyPIDcontrol.h"
#include <iomanip>
#ifdef _DEBUG
#define new DEBUG_NEW
#endif
// 唯一的应用程序对象
CWinApp theApp;
using namespace std;
int _tmain(int argc, TCHAR* argv[], TCHAR* envp[])
{
int nRetCode = 0;
// 初始化 MFC 并在失败时显示错误
if (!AfxWinInit(::GetModuleHandle(NULL), NULL, ::GetCommandLine(), 0))
{
// TODO: 更改错误代码以符合您的需要
_tprintf(_T("致命错误: MFC 初始化失败\n"));
nRetCode = 1;
}
else
{
// TODO: 在此处为应用程序的行为编写代码。
double simTime=0.001;
int ctype=2;
greyPIDcontrol gpc(nNumRow,nNumColoumn);
CMatrix data=gpc.initDataMatrix(simTime);
gpc.selectControllerType(ctype);
CMatrix x1=gpc.AGOmatrix(data);
CMatrix B=gpc.dataMatrixB(x1);
// for(int i=0;i<nNumColoumn;i++)
//{ for(int j=0;j<nNumRow+1;j++)
// {
// double b=B.GetElement(i,j);
// std::cout<<setw(10)<<b<<"\t";
// }
// std::cout<<std::endl;
//}
gpc.data_Dk(data,simTime);
gpc.AGO_D1k(gpc.D);
gpc.paramaterVectorEstimation(B);
FILE *fp;
if(!(fp=fopen("error2.txt","w")))
printf("creat file failed!");
gpc.x[0]=0,gpc.x[1]=0;
for(int i=0;i<2000;i++)
{
gpc.greyCompensationControl(simTime,i);
fprintf(fp,"%f\t%f\t%f\t%f\t%f\t\n",i*simTime,gpc.r[i],gpc.x[0],gpc.r[i]-gpc.x[0],gpc.u);
}
}
return nRetCode;
}
//--------------灰色PID控制器构造函数------------------
// 函数名 ： greyPIDcontrol
// 入口参数： nRows 初始离散数列行数
// nCols 初始离散数列数
// 返回值 ： NULL
//-----------------------------------------------------
greyPIDcontrol::greyPIDcontrol(int nRows,int nCols)
{
nNumRows=nRows;
nNumCols=nCols;
x[0]=0,x[1]=0;
V[0]=0,V[1]=0,V[2]=0;
simStep=0;
step=0;
}
/////////////////////////////////////////////////////////////////////////////////////
// 灰色PID控制算法的第一步 //
// 灰色估计器对不确定部分的模型参数建立GM (0,N)模型的具体算法的步骤 //
//////////////////////////////////////////////////////////////////////////////////////
//--------------------------建立原始离散序列-----------------------------step 1
// 函数名 : initDataMatrix
// 入口参数: ts 采样时间
// 返回值 : CMatrix(数据矩阵)
//------------------------------------------------------------------------
CMatrix greyPIDcontrol::initDataMatrix(double ts)
{
double value[nSize];
int count=0;
int k=2;
for(int i=0;i<nNumCols;i++)
{
rungeKutta4(x,k,ts,i);
value[count]=x[0],value[count+nNumCols]=x[1];
count++;
}
CMatrix data(nNumRows,nNumCols,value);
return data;
}
//--------------------------选择控制器的类型--------------------------------
// 函数名 : selectControllerType
// 入口参数: type 控制器类型(1=PID控制器,2=灰色PID控制器)
// 返回值 : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::selectControllerType(int type)
{
M=type;
}
//-----------------计算一次累加生成(1-AGO)离散序列x1(k)------------------step 2
// 函数名 : AGOmatrix
// 入口参数: mat 输入矩阵
// 返回值 : CMatrix(输出矩阵)
//------------------------------------------------------------------------
CMatrix greyPIDcontrol::AGOmatrix(CMatrix mat)
{
double a[nSize];
double *pData=new double[nSize];
for(int i=0;i<nNumRows;i++)
for(int j=0;j<nNumCols;j++)
{
a[j+i*nNumCols]=mat.GetElement(i,j);
}
for(int i=0;i<nNumRows;i++)
{ double sum=0;
for(int j=0;j<nNumCols;j++)
{
sum+=a[j+i*nNumCols];
pData[j+i*nNumCols]=sum;
}
}
CMatrix data1(nNumRows,nNumCols,pData);
delete[] pData;
return data1;
}
//---------------------------计算数据矩阵B-------------------------------step 3
// 函数名 : dataMatrixB
// 入口参数: mat 输入矩阵
// 返回值 : CMatrix(输出矩阵)
//------------------------------------------------------------------------
CMatrix greyPIDcontrol::dataMatrixB(CMatrix mat)
{
double pData[NSize];
int count=0;
double N=1;
CMatrix matT(nNumCols,nNumRows);
matT=mat.Transpose();
for(int i=0;i<nNumCols;i++)
{
for(int j=0;j<nNumRows;j++)
{
pData[count++]=matT.GetElement(i,j);
}
pData[count++]=N++;
}
CMatrix data2(nNumCols,nNumRows+1,pData);
return data2;
}
//----------------------计算离散序列D(k)---------------------------------step 4
// 函数名 : data_Dk
// 入口参数: mat 输入矩阵
// ts 采样时间
// 返回值 : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::data_Dk(CMatrix mat,double ts)
{
double a21=0,a22=-25,b=133;
double kp=30,kd=5;
double dx2[nNumColoumn];
dx2[0]=(mat.GetElement(1,0)-0)/ts;
for(int k=1;k<nNumCols;k++)
{
dx2[k]=(mat.GetElement(1,k)-mat.GetElement(1,k-1))/ts;
}
for(int k=0;k<nNumCols;k++)
{
double up=b*kp*(r[k]-mat.GetElement(0,k))+b*kd*(dr[k]-mat.GetElement(1,k));
D[k]=(dx2[k]-a21*mat.GetElement(0,k)-a22*mat.GetElement(1,k)-up)/b;
//std::cout<<D[k]<<std::endl;
}
}
//-------------------计算(1-AGO)离散序列D1(k)----------------------------step 5
// 函数名 : AGO_D1k
// 入口参数: value[] 离散系列Dk
// 返回值 : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::AGO_D1k(double value[])
{
D1[0]=value[0];
double sum=0;
for(int k=0;k<nNumCols;k++)
{
sum+=value[k];
D1[k]=sum;
// std::cout<<D1[k]<<std::endl;
}
}
//----------------估算不确定部分bD(x,t)的灰色模型的参数向量V-------------
// 函数名 : paramaterVectorEstimation
// 入口参数: mat 输入矩阵
// 返回值 : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::paramaterVectorEstimation(CMatrix mat)
{
CMatrix BT(nNumRows+1,nNumCols);
CMatrix BTB(nNumRows+1,nNumRows+1);
CMatrix BBB(nNumRows+1,nNumCols);
BT=mat.Transpose();
BTB=BT*mat;
BTB.InvertGaussJordan();
BBB=BTB*BT;
for(int i=0;i<nNumRows+1;i++)
{ double sum=0;
for(int j=0;j<nNumCols;j++)
{
sum+=BBB.GetElement(i,j)*D1[j];
}
V[i]=sum;
std::cout<<V[i]<<std::endl;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
// 灰色PID控制算法的第二步(补偿控制) //
//////////////////////////////////////////////////////////////////////////////////////////////
//-------------------------灰色补偿控制-----------------------------
// 函数名 :greyCompensationControl
// 入口参数:
// 返回值 :NULL
//------------------------------------------------------------------
void greyPIDcontrol::greyCompensationControl(double ts,int step)
{
int k=2;
rungeKutta4(x, k,ts,step);
}
//--------------------------被控对象的状态方程形式------------------------
// 函数名 : paramaterVectorEstimation
// 入口参数: dx[] 方程组导数项
// x[] 方程组常数项
// h 采样时间
// M 灰色补偿控制参数选择(M=1 无补偿控制;M=2,有补偿控制)
// 返回值 : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::dynamicModel(double dx[],double x[],double h,int step)
{
double a21=0,a22=-25,b=133;
double kp=30,kd=5;
r[step]=0.5*sin(2*pi*h*step);
dr[step]=0.5*2*pi*cos(2*pi*h*step);
double e=r[step]-x[0];
double de=dr[step]-x[1];
double up=kp*e+kd*de;
double V1[2]={5,5},f=5;
double DD=V1[0]*x[0]+V1[1]*x[1]+f;
double uc=0;
if(M==1)
uc=0; //No Grey Compensation
else if(M==2)
uc=-V[0]*x[0]-V[1]*x[1]-V[2]; //Grey Compensation
u=up+uc;
dx[0]=x[1];
dx[1]=a21*x[0]+a22*x[1]+b*(u+DD);
}
//---------------------四阶龙格库塔解微分方程-----------------------------
// 函数名 : paramaterVectorEstimation
// 入口参数: x[] 方程组常数项
// k 方程组个数
// h 采样时间
// 返回值 : NULL
//------------------------------------------------------------------------
void greyPIDcontrol::rungeKutta4(double x[],int k,double h,int step)
{
double a[4], *b, *f;
b = new double[k];
f = new double[k];
//double a[4],b[2]={0},f[2]={0};
a[0] = h/2.0; a[1] = a[0]; a[2] = h; a[3] = h;
dynamicModel(f, x,h,step);
for (int i = 0; i <=k-1; i++) b[i] = x[i];
for (int j = 0; j <=2; j++)
{
for (i = 0; i <=k-1; i++)
{
x[i] += a[j]*f[i];
b[i] += a[j+1]*f[i]/3.0;
}
dynamicModel(f, x,h,step);
}
for (i = 0; i <=k-1; i++) x[i] = b[i]+h*f[i]/6.0;//解出微分方程组
delete(b); delete(f); //
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -