lagrange.cpp

工程优化和数值计算中常用的优化程序

#include "iostream.h"
#include "math.h"

typedef double (*FUN) (double *);	// 自定义的函数指针类型


//函数声明
void comput_grad(double (*pf)(double *x),  int n,  double *point,  double *grad);			//计算梯度
double line_search(double (*pf)(double *x),   int n,  double *start,  double *direction);	//解析法线搜索
double DFP(double (*pf)(double *x),   int n,   double *min_point);		//无约束变尺度法
double LAGRANGE(FUN obj,FUN *pG_group,FUN *pH_group,int var_num,int g_num,int h_num,double *min_point);

////////////////////////////////////////////////////////////////////////////////
//目标函数
double obj(double *x)
{
	return 3*(x[0]-1)*(x[0]-1)+2*(x[1]-2)*(x[1]-2)+(x[2]-3)*(x[2]-3);
}

//不等式约束
double g1(double *x)
{
	return -x[0];
}

double g2(double *x)
{
	return -x[1];
}

double g3(double *x)
{
	return -x[2];
}

//等式约束
double h1(double *x)
{
	return x[0]+x[1]+x[2]-1;
}

double h2(double *x)
{
	return -2*x[0]+3*x[1]+x[2]-2;
}

//前面带下划线的变量 注意使用
int _h_num=2;		//等式约束数目
int _g_num=3;		//不等式约束数目

double *_u=new double[_g_num];		//惩罚因子
double *_lamda=new double[_h_num];	//lagrange乘子
double _M;

FUN _H[2]={h1,h2};
FUN _G[3]={g1,g2,g3};		//


///////////////////////////////////////////////////////////////////////////////////////
//LAGRANGE乘子法用到的函数
//
double newObj(double *x)
{
	double tmp=0.0;
	int i;

	tmp=tmp+obj(x);
	for(i=0;i<_h_num;i++)
	{
		tmp=tmp+_lamda[i]*_H[i](x)+0.5*_M*_H[i](x)*_H[i](x);
	}

	for(i=0;i<_g_num;i++)
	{
		if ( (_G[i](x)+_u[i]/_M)>0 )
			tmp=tmp+0.5*_M*(_G[i](x)+_u[i]/_M)*(_G[i](x)+_u[i]/_M);
	}
	return tmp;
}



/////////////////////////////////////////////////////////////////////////////////////////
//梯度计算模块
//参数：指向目标函数的指针，变量个数，求梯度的点，结果
void comput_grad(double (*pf)(double *x),
				  int n,
				  double *point,
				  double *grad)
{
	double h=1E-3;
	int i;
	double *temp;
	temp = new double[n];
	for(i=1;i<=n;i++)
	{
		temp[i-1]=point[i-1];
	}
	for(i=1;i<=n;i++)
	{
		temp[i-1]+=0.5*h;
		grad[i-1]=4*pf(temp)/(3*h);
		temp[i-1]-=h;
		grad[i-1]-=4*pf(temp)/(3*h);
		temp[i-1]+=(3*h/2);
		grad[i-1]-=(pf(temp)/(6*h));
		temp[i-1]-=(2*h);
		grad[i-1]+=(pf(temp)/(6*h));
		temp[i-1]=point[i-1];
	}
	delete[] temp;
}

/////////////////////////////////////////////////////////////////////////////////////////
//一维搜索模块
//参数：指向目标函数的指针，变量个数，出发点，搜索方向
//返回：最优步长
double line_search(
				   double (*pf)(double *x),
				   int n,
				   double *start,
				   double *direction)
{
	int i;
	double step=0.001;
	double a=0,value_a,diver_a;
	double b,value_b,diver_b;
	double t,value_t,diver_t;
	double s,z,w;
	double *grad,*temp_point;

	grad=new double[n];
	temp_point=new double[n];
	comput_grad(pf,n,start,grad);
	diver_a=0;
	for(i=1;i<=n;i++)
		diver_a=diver_a+grad[i-1]*direction[i-1];
	do
	{
		b=a+step;
		for(i=1;i<=n;i++)
			temp_point[i-1]=start[i-1]+b*direction[i-1];
		comput_grad(pf,n,temp_point,grad);
		diver_b=0;
		for(i=1;i<=n;i++)
			diver_b=diver_b+grad[i-1]*direction[i-1];
		if( fabs(diver_b)<1E-10 )
		{
			delete[] grad;
			delete[] temp_point;
			return(b);
		}
		if( diver_b<-1E-15 )
		{
			a=b;
			diver_a=diver_b;
			step=2*step;
		}
	}while( diver_b<=1E-15 );

	for(i=1;i<=n;i++)
		temp_point[i-1]=start[i-1]+a*direction[i-1];
	value_a=(*pf)(temp_point);
	for(i=1;i<=n;i++)
		temp_point[i-1]=start[i-1]+b*direction[i-1];
	value_b=(*pf)(temp_point);

	do
	{
		s=3*(value_b-value_a)/(b-a);
		z=s-diver_a-diver_b;
		w=sqrt( fabs(z*z-diver_a*diver_b) );	//////////////////!!!!!!!!!!!!!!!!!!!!!!
		t=a+(w-z-diver_a)*(b-a)/(diver_b-diver_a+2*w);
		value_b=(*pf)(temp_point);
		for(i=1;i<=n;i++)
			temp_point[i-1]=start[i-1]+t*direction[i-1];
		value_t=(*pf)(temp_point);
		comput_grad(pf,n,temp_point,grad);
		diver_t=0;
		for(i=1;i<=n;i++)
			diver_t=diver_t+grad[i-1]*direction[i-1];
		if(diver_t>1E-6)
		{
			b=t;
			value_b=value_t;
			diver_b=diver_t;
		}
		else if(diver_t<-1E-6)
		{
			a=t;
			value_a=value_t;
			diver_a=diver_t;
		}
		else break;
	}while( (fabs(diver_t)>=1E-6) && (fabs(b-a)>1E-6) );

	delete[] grad;
	delete[] temp_point;
	return(t);

}

/////////////////////////////////////////////////////////////////////////////////////////
//无约束变尺度法DFP函数声明
//
//参数：pf指向目标函数的指针，n变量个数，min_point接受初始点、存放结果
//返回：极小点处函数值
//
double DFP(
		   double (*pf)(double *x),
		   int n,
		   double *min_point
		   )
{
	int i,j;
	int k=0;
	double e=1E-6;
	double g_norm;
		
	double *g0=new double[n];		//梯度
	double *g1=new double[n];
	double *dg=new double[n];

	double *p=new double[n];		//搜索方向 =-g
	double t;						//一维搜索步长

	double *x0=new double[n];
	double *x1=new double[n];
	double *dx=new double[n];

	double **H=new double*[n];
	for (i=0; i<n; i++)		H[i] = new double[n];

	double **tempH=new double*[n];
	for (i=0; i<n; i++)		tempH[i] = new double[n];

	double *gH=new double[n];
	double *Hg=new double[n];
	double num1;
	double num2;



	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			if(i==j)	H[i][j]=1.0;	// H0=I
			else	H[i][j]=0.0;
			tempH[i][j]=0.0;
		}


	for(i=0;i<n;i++)
		x0[i]=min_point[i];	

	comput_grad(pf,n,x0,g0);

	g_norm=0.0;
	for(i=0;i<n;i++)	g_norm=g_norm+g0[i]*g0[i];	
	g_norm=sqrt(g_norm);
	if (g_norm<e) 
	{
		for(i=0;i<n;i++)	min_point[i]=x0[i];

		delete[] g0;		
		delete[] g1;
		delete[] dg;
		delete[] p;
		delete[] x0;
		delete[] x1;
		delete[] dx;
		for (i=0; i<n; i++)		delete[] H[i];
		delete []H;
		for (i=0; i<n; i++)		delete[] tempH[i];
		delete []tempH;
		delete[] gH;
		delete[] Hg;

		return pf(min_point);
	}

	for(i=0;i<n;i++)	p[i]=-g0[i];	

	do
	{
		t=line_search(pf,n,x0,p);				
		for(i=0;i<n;i++)	x1[i]=x0[i]+t*p[i];
 		comput_grad(pf,n,x1,g1);
		g_norm=0.0;
		for(i=0;i<n;i++)	g_norm=g_norm+g1[i]*g1[i];
		g_norm=sqrt(g_norm);
		//cout<<k<<"    "<<x0[0]<<"       "<<x0[1]<<"       "<<g_norm<<"\n";
		if (g_norm<e) 
		{
			for(i=0;i<n;i++)	min_point[i]=x1[i];

			delete[] g0;		
			delete[] g1;
			delete[] dg;
			delete[] p;
			delete[] x0;
			delete[] x1;
			delete[] dx;
			for (i=0; i<n; i++)		delete[] H[i];
			delete []H;
			for (i=0; i<n; i++)		delete[] tempH[i];
			delete []tempH;
			delete[] gH;
			delete[] Hg;
			
			return pf(min_point);
		}
		for(i=0;i<n;i++)
		{
			dx[i]=x1[i]-x0[i];
			dg[i]=g1[i]-g0[i];
		}

		//////////////////求Hk+1的矩阵运算

		//g*H,H*g
		for(i=0;i<n;i++)
		{
			gH[i]=0.0;
			Hg[i]=0.0;
		}
		for(i=0;i<n;i++)
		{
			for(j=0;j<n;j++)
			{
				gH[i]=gH[i]+dg[j]*H[j][i];
				//Hg[i]=Hg[i]+H[i][j]*dg[j];
				Hg[i]=gH[i];
			}			
		}

		//num1,num2
		num1=0.0;
		num2=0.0;
		for(i=0;i<n;i++)
		{
			num1=num1+dx[i]*dg[i];
			num2=num2+gH[i]*dg[i];
		}

		//tempH[i][j]
		for(i=0;i<n;i++)
			for(j=0;j<n;j++)
				tempH[i][j]=0.0;
		for(i=0;i<n;i++)
		{
			for(j=0;j<n;j++)
			{
				tempH[i][j]=tempH[i][j]+H[i][j];
				tempH[i][j]=tempH[i][j]+dx[i]*dx[j]/num1;
				tempH[i][j]=tempH[i][j]-Hg[i]*gH[j]/num2;
			}
		}

		for(i=0;i<n;i++)
		{
			for(j=0;j<n;j++)
			{
				H[i][j]=tempH[i][j];
			}
		}
		/////////////////////////////

		//P 
		for(i=0;i<n;i++)	p[i]=0.0;
		for(i=0;i<n;i++)
		{
			for(j=0;j<n;j++)
			{
				p[i]=p[i]-H[i][j]*g1[j];
			}			
		}

		for(i=0;i<n;i++)
		{
			g0[i]=g1[i];
			x0[i]=x1[i];
		}
		k=k+1;
	}while(g_norm>e);

	for(i=0;i<n;i++)	min_point[i]=x1[i];

	delete[] g0;		
	delete[] g1;
	delete[] dg;
	delete[] p;
	delete[] x0;
	delete[] x1;
	delete[] dx;
	for (i=0; i<n; i++)		delete[] H[i];
	delete []H;
	for (i=0; i<n; i++)		delete[] tempH[i];
	delete []tempH;
	delete[] gH;
	delete[] Hg;

	return pf(min_point);
}

//////////////////////////////////////////////////////////////////////////////////////////
//lagrange 乘子法
double LAGRANGE(
				FUN obj,
				FUN *pG_group,
				FUN *pH_group,
				int var_num,
				int g_num,
				int h_num,
				double *min_point
				)
{

	double *X=new double[var_num];
	double alpha;
	double err;
	double e;

	int i;
	int k;
	
	//初始化
	for(i=0;i<var_num;i++)	X[i]=1;
	for(i=0;i<g_num;i++)	_u[i]=1;
	for(i=0;i<h_num;i++)	_lamda[i]=1;
	_M=1;
	alpha=2;
	e=1E-5;
	k=0;
	
	do{
		DFP(newObj, var_num, X);

		////////计算error
		err=0.0;
		for(i=0;i<g_num;i++)
		{
			if ( _G[i](X)>(-_u[i]/_M) )	err=err+_G[i](X)*_G[i](X);
			else	err=err+(_u[i]/_M)*(_u[i]/_M);
		}
		for(i=0;i<h_num;i++)		err=err+_H[i](X)*_H[i](X);
		
		//更新参数//////////////
		for(i=0;i<g_num;i++)
		{
			if( (_u[i]+_M*_G[i](X)) >0 )	_u[i]=_u[i]+_M*_G[i](X);
			else	_u[i]=0;
		}
		for(i=0;i<h_num;i++)		_lamda[i]=_lamda[i]+_M*_H[i](X);

		_M=alpha*_M;
		k=k+1;
		//cout<<k<<"   "<<err<<"\n";

	}while(err>e);

	for(i=0;i<var_num;i++)	min_point[i]=X[i];

	delete[] X;
	delete[] _u;
	delete[] _lamda;

	return obj(min_point);
}


//////////////////////////////////////////////////////
void main()
{
	int var_num=3;
	double min_point[3]={0,0,0};
	double min=LAGRANGE(obj,_G,_H,var_num,_g_num,_h_num,min_point);

	cout<<min_point[0]<<"   "<<min_point[1]<<"   "<<min_point[2]<<" \n";
	cout<<min;

	
	
}