% 連桿機構實現函數的優化設計
% (調用目標函數jfg_f和非線性約束函數cdj_g)
% 1----優化設計主程序
% 設計變量的初始值和已知桿件長度(曲柄和機架)
x0=[4.5;4];
qb=1;jj=5;
% 設計變量的下界與上界
lb=[1;1];
ub=[10;10];
% 線性不等式約束(g1(x)和g2(x))中設計變量的系數矩陣
a=zeros(2,2);
a(1,1)=-1;a(2,1)= 1;
a(3,2)=-1;a(4,2)= 1;
% 線性不等式約束(g1(x)和g2(x))中的常數項列陣
b=[-1;10;-1;10];
% 使用多維約束優化命令fmincon(調用目標函數jfg_f和非線性約束函數cdj_g)
% 沒有等式約束,參數Aeq和beq定義為空矩陣符號“[ ]”
[x,fn]=fmincon(@jfg_f,x0,a,b,[],[],lb,ub,@cdj_g);
disp '     ********  連桿機構實現函數優化設計最優解  ********'  
fprintf (1,'             連桿相對長度             a = %3.4f  \n',x(1))
fprintf (1,'             搖桿相對長度             b = %3.4f  \n',x(2))
fprintf (1,'             輸出角平方誤差之和      f* = %3.4f  \n',fn)
% 調用多維約束優化非線性約束函數(cdj_g)計算最優點x*的性能約束函數值 
g=cdj_g(x);
disp '        ========    最優點的性能約束函數值    ========'
fprintf (1,'             最小傳動角約束函數值   g1* = %3.4f \n',g(1))
fprintf (1,'             最大傳動角約束函數值   g2* = %3.4f \n',g(2))

% 2----連桿機構實現函數優化的目標函數(jfg_f)
function f=jfg_f(x);
s=50;qb=1;jj=5;fx=0;
fa0=acos(((qb+x(1))^2-x(2)^2+jj^2)/(2*(qb+x(1))*jj));     % 曲柄初始角
pu0=acos(((qb+x(1))^2-x(2)^2-jj^2)/(2*x(2)*jj));          % 搖桿初始角
for i=1:s
    fai=fa0+0.5*pi*i/s;
    pui=pu0+2*(fai-fa0)^2/(3*pi);
    ri=sqrt(qb^2+jj^2-2*qb*jj*cos(fai));
    alfi=acos((ri^2+x(2)^2-x(1)^2)/(2*ri*x(2)));
    bati=acos((ri^2+jj^2-qb^2)/(2*ri*jj));
    if fai>0 & fai<=pi
        psi=pi-alfi-bati;
    elseif  fai>pi & fai<=2*pi
        psi=pi-alfi+bati;
    end
    fx=fx+(pui-psi)^2;                            % 輸出角平方誤差之和
end
f=fx;

% 3----連桿機構實現函數優化的非線性不等式約束函數(cdj_g)
function [g,ceq]=cdj_g(x);
qb=1;jj=5;
gamn=45*pi/180;gamm=135*pi/180;
g(1)=x(1)^2+x(2)^2-(jj-qb)^2-2*x(1)*x(2)*cos(gamn);   % 最小傳動角約束
g(2)=-x(1)^2-x(2)^2+(jj+qb)^2-2*x(1)*x(2)*cos(gamn);  % 最大傳動角約束
ceq=[];