clear;
clc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%优化问题%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x=zeros(2,1);
x(1)=2; %初值点赋值
x(2)=2;
hmax=4;
hmin=1;
hx=x(1)^2+x(2)^2-6*x(1)-2*x(2)+10;%%不等式约束
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%变量初始化
%%%%%%%%%%原变量初始化
l=(1-0.25)*(hmax-hmin);
if l>max(0.25*(hmax-hmin),hx-hmin)
l=max(0.25*(hmax-hmin),hx-hmin);
end
u=(hmax-hmin)-l;
%%%%%%%%%%%对偶变量初始化
%拉格朗日乘子 z,w,y的初始值
miu=0.1;
z=miu/l;%拉格朗日乘子
w=miu/u-z;%拉格朗日乘子 z+w>0即可
y=zeros(1);%拉格朗日乘子
%%%%%%%%%%%%%%%%%%%%%%%%%%%main function%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k=0;
while k<50
Gap=l*z+u*(z+w);
if Gap>1e-4
%%%%%%%%%目标函数矩阵
%%%%%%%%%一阶海森矩阵
Pfx1=zeros(2,1);
Pfx1(1)=2*x(1)-4;
Pfx1(2)=2*x(2)-8;
%%%%%%%%%二阶海森矩阵
Pfx2=2*eye(2,2);
%%%%%%%%%等式约束矩阵
%%%%%%%%%一阶雅可比矩阵
Jgx1=zeros(1,2);
Jgx1(1,1)=2*x(1)-2;
Jgx1(1,2)=2*x(2)-2;
%%%%%%%%%二阶海森矩阵
Pgx2=2*eye(2,2);
%%%%%%%%%不等式约束矩阵
%%%%%%%%%一阶雅可比矩阵
Jhx1=zeros(1,2);
Jhx1(1,1)=2*x(1)-6;
Jhx1(1,2)=2*x(2)-2;
%%%%%%%%%二阶海森矩阵
Phx2=2*eye(2,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%求解常数项%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Lz
Lz=-l-u+hmax-hmin;
%Lw
Lw=-(x(1)^2+x(2)^2-6*x(1)-2*x(2)+10)-u+hmax;
%Ll
Ll=-l*z+miu;
%Lu
Lu=-u*(z+w)+miu;
%%%%%%%%%%%%%%求解修正量
%%%%%%%%%%%%%%%%%%%%Jd%%%%%%%%%%%%%%%%%%%%
Jd=Pfx2-y*Pgx2+w*Phx2+Jhx1'*((z+w)/u+z/l)*Jhx1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%% LXX %%%%%%%%%%%%%%%%%%%%
Ly=x(1)^2+x(2)^2-2*x(1)-2*x(2)-2;
Lx=-Pfx1+Jgx1'*y-Jhx1'*w;
Lxx=Lx-Jhx1'*((Lu-(z+w)*Lw)/u-(Ll-z*Lz+z*Lw)/l);
%%%%%%%%%%%%%% H %%%%%%%%%%%%%%%%%%%%
H=[Jd,-Jgx1';-Jgx1,zeros(1,1)];
LxLy=[Lxx;Ly];
I=eye(2+1);
dxdy=I/H*LxLy;
%%%%%dx
dx=dxdy(1:2);
%%%%%dy
dy=dxdy(3);
%%%%%du
du=-Jhx1*dx+Lw;
%%%%%dl
dl=Lz-du;
%dz
dz=(Ll-z*dl)/l;
%dw
dw=(Lu+(z+w)*(Jhx1*dx-Lw))/u-(Ll-z*(Lz+Jhx1*dx-Lw))/l;
%%%%%%%%%%%%更新障碍参数(扰动因子)
miu=0.2*Gap/2;
%计算步长
alfap=1;
alfad=1;
if dl<0&&-0.9995*l/dl<alfap
alfap=-0.9995*l/dl;
end
if du<0&&-0.9995*u/du<alfap
alfap=-0.9995*u/du;
end
if dz<0&&-0.9995*z/dz<alfad
alfad=-0.9995*z/dz;
end
if dz+dw<0&&-0.9995*(z+w)/(dz+dw)<alfad
alfad=-0.9995*(z+w)/(dz+dw);
end
%
% alfap=0.9995*alfap;%安全系数
% alfad=0.9995*alfad;
x=x+alfap*dx;
l=l+alfap*dl;
u=u+alfap*du;
y=y+alfad*dy;
z=z+alfad*dz;
w=w+alfad*dw;
%%%%输出%%%%%%%%%%%%%%%%%%%
k=k+1;
Y=[dl;du;dz;dw;dx(1);dx(2);dy];
else
break
end
end
k
Gap
x
fx=x(1)^2+x(2)^2-4*x(1)-8*x(2)+20%%%目标函数
gx=x(1)^2+x(2)^2-2*x(1)-2*x(2)-2%%%%等式约束
hx=x(1)^2+x(2)^2-6*x(1)-2*x(2)+10%%%不等式约束