nlp.cpp

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

#include "stdafx.h"
//#include "iostream.h"
#include "nlp.h"
#include "math.h"
#include "matrix.h"

//lagrange乘子法用到的参数
double *_u=0;		//惩罚因子
double *_lamda=0;	//lagrange乘子
double _M;


int _h_num;		//等式约束数目
int _g_num;		//不等式约束数目
FUN _obj;		//目标函数  min f(x)
FUN *_H=0;		//等式约束  hi(x)=0
FUN *_G=0;		//不等式约束 gi(x)<=0

///////////////////////////////////////////////////////////////////////////////////////
//LAGRANGE乘子法用到的函数
//
double newObj(double *x)
{
	double tmp=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=0;
	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];
	}
	if(temp)	{delete[] temp;	temp=0;}
}

/////////////////////////////////////////////////////////////////////////////////////////
//一维搜索模块
//参数：指向目标函数的指针，变量个数，出发点，搜索方向
//返回：最优步长
double line_search(
				   double (*pf)(double *x),
				   int n,
				   double *start,
				   double *direction)
{
	int k;
	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=0,*temp_point=0;

	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];

	k=0;
	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 )
		{
			if(grad)	{delete[] grad;		grad=0;}
			if(temp_point)	{delete[] temp_point;	temp_point=0;}
			return(b);
		}
		if( diver_b<-1E-15 )
		{
			a=b;
			diver_a=diver_b;
			step=2*step;
		}
		k++;
	}while( (diver_b<=1E-15) && (k<10000));

	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);

	k=0;
	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;
		k++;
	}while( (fabs(diver_t)>=1E-6) && (fabs(b-a)>1E-6) && (k<10000));

	if(grad)	{delete[] grad;		grad=0;}
	if(temp_point)	{delete[] temp_point;	temp_point=0;}
	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-5;
	double g_norm;
	double t;						//一维搜索步长
		
	double *g0=new double[n];		//梯度
	double *g1=new double[n];
	double *x0=new double[n];
	double *x1=new double[n];
	double *p=new double[n];		//搜索方向 =-g

	CMatrix dg(n,1);
	CMatrix dx(n,1);
	CMatrix H(n,n),tempH(n,n);
	H.ZeroMatrix();
	tempH.ZeroMatrix();
	for(i=0;i<n;i++)	H[i][i]=1;
	
	//-------------------------------------------------------------------
	for(i=0;i<n;i++)	x0[i]=min_point[i];	
	comput_grad(pf,n,x0,g0);
	g_norm=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];
		if(g0)	{delete[] g0;	g0=0;}
		if(g1)	{delete[] g1;	g1=0;}
		if(x0)	{delete[] x0;	x0=0;}
		if(x1)	{delete[] x1;	x1=0;}
		if(p)	{delete[] p;	p=0;}
		
		return pf(min_point);
	}

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

	//--------------------loop----------------------------------
	k=0;
	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);
		
		if (g_norm<e) 
		{			
			for(i=0;i<n;i++)	min_point[i]=x1[i];
			if(g0)	{delete[] g0;	g0=0;}
			if(g1)	{delete[] g1;	g1=0;}
			if(x0)	{delete[] x0;	x0=0;}
			if(x1)	{delete[] x1;	x1=0;}
			if(p)	{delete[] p;	p=0;}

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

		//////////////////求Hk+1的矩阵运算
		
		CMatrix dxt(dx);
		CMatrix dgt(dg);
		dxt.ReverseMatrix();
		dgt.ReverseMatrix();

		tempH.ZeroMatrix();	
		tempH=H+dx*dxt/(dxt*dg)[0][0]-H*dg*dgt*H/(dgt*H*dg)[0][0];

		H=tempH;
		/////////////////////////////

		//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) && (k<2000));

	for(i=0;i<n;i++)	min_point[i]=x1[i];
	if(g0)	{delete[] g0;	g0=0;}
	if(g1)	{delete[] g1;	g1=0;}
	if(x0)	{delete[] x0;	x0=0;}
	if(x1)	{delete[] x1;	x1=0;}
	if(p)	{delete[] p;	p=0;}
	
	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=1E-5;

	int i;
	int k;
	
	//初始化-----------------------------------------
	_g_num=g_num;
	_h_num=h_num;

	_obj=obj;
	_G=new FUN[g_num];
	_H=new FUN[h_num];
	for(i=0;i<g_num;i++)	_G[i]=pG_group[i];
	for(i=0;i<h_num;i++)	_H[i]=pH_group[i];

	
	_u=new double[_g_num];
	_lamda=new double[_h_num];

	for(i=0;i<var_num;i++)	X[i]=min_point[i];
	for(i=0;i<g_num;i++)	_u[i]=1;
	for(i=0;i<h_num;i++)	_lamda[i]=1;
	_M=1;
	alpha=2;

	//--------------main loop-----------------------------
	k=0;
	do{
		DFP(newObj, var_num, X);

		//计算err
		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) && (k<2000));

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

	if(X)	{delete[] X;	X=0;}
	if(_u) 	{delete[] _u; 		_u=0;}
	if(_lamda)	{delete[] _lamda;	_lamda=0;}
	if(_H)	{delete[] _H;	_H=0;}
	if(_G)	{delete[] _G;	_G=0;}

	return obj(min_point);
}