clear
D=[50,7,500];%依次表示行,列,頁
N=3;n=8;%依次表示軸數(shù)和插值點數(shù)
%以P表示位置,V表示速度,A表示加速度,J表示加加速度
%T表示時間,h表示時間間隔
VC=[100,95,100];%依次設(shè)置第一軸,第二軸,第三軸的速度約束
AC=[70,75,75];%依次設(shè)置第一軸,第二軸,第三軸的加速度約束
JC=[70,85,70];%依次設(shè)置第一軸,第二軸,第三軸的沖擊約束
V=zeros(N,n);%設(shè)置速度邊界
A=zeros(N,n);%設(shè)置加速度邊界
KT=1;KJ=0;
t1=0;%起始時間
Ps=0.04;Pm=0.16;Pc=0.8;
P(1,1)=120;P(1,3)=100;P(1,4)=60;P(1,5)=30;P(1,6)=10;P(1,8)=0;
P(2,1)=-10;P(2,3)=20;P(2,4)=0;P(2,5)=30;P(2,6)=60;P(2,8)=80;
P(3,1)=0;P(3,3)=-20;P(3,4)=10;P(3,5)=20;P(3,6)=50;P(3,6)=70;
%以上三行設(shè)置初始位置點

%群體數(shù)據(jù)的初始化
%rand('state',sum(50*clock)); 
rand('state',1);
JUST=1;
while JUST<=D(1)
    %確定時間間隔及時間表
    for i=1:n-1;
        H(i)=rand*5;
    end;%h(i)表示各插值點的時間間隔
    T_ini=SOV_T(H,n,t1);
    %T(i)各插值點的即時時間
    %判斷點是否生成
   QA=SOL_QA(H,V,A,P,N,n); %QA為N*n矩陣
    BDVC=JUST_VC(QA,T_ini,H,P,VC,N,n);
    if BDVC==1;
        BDAC=JUST_AC(QA,AC,N,n);
        if BDAC==1;
           BDJC=JUST_JC(QA,JC,H,N,n);
            if BDJC==1;
                HZD(JUST,:,1)=H;%HZD表示所有個體
                JUST=JUST+1;
            end;
        end;
    end;
end;
DS=1;
while DS<=D(3)
    %HD=HZD(:,:,1);%HD表示某一代群體
    for ii=1:D(1);
       QAD(:,:,ii)=SOL_QA(HZD(ii,:,DS),V,A,P,N,n);%群體中的三軸加速度值
    end;
    %QADM=SOL_QAM(QAD,N,n,D(1));
    FITD=SOV_fitness(HZD(:,:,DS),QAD,KT,KJ,N,n,D(1));
   %以下是交叉操作
   JCLS=1;
   while JCLS<=D(1)*Pc/2;
        XZ_new1=rand;XZ_new2=rand;%選擇
        [XB_new1,XB_new2]=SEAR_XZXB(FITD,XZ_new1,XZ_new2,D(1));
        [GT_new1,GT_new2]=CRE_JC(HZD(XB_new1,:,DS),HZD(XB_new2,:,DS),D(2));
        QA_new1=SOL_QA(GT_new1,V,A,P,N,n);
        QA_new2=SOL_QA(GT_new2,V,A,P,N,n);
        T_new1=SOV_T(GT_new1,n,t1);
        T_new2=SOV_T(GT_new2,n,t1);
        BDVC_new1=JUST_VC(QA_new1,T_new1,GT_new1,P,VC,N,n);
        BDVC_new2=JUST_VC(QA_new2,T_new2,GT_new2,P,VC,N,n);
        if BDVC_new1==1&&BDVC_new2==1;
            BDAC_new1=JUST_AC(QA_new1,AC,N,n);
            BDAC_new2=JUST_AC(QA_new2,AC,N,n);
            %chsj_1=1;
            if BDAC_new1==1&&BDAC_new2==1;
                 BDJC_new1=JUST_JC(QA_new1,JC,GT_new1,N,n);
                 BDJC_new2=JUST_JC(QA_new2,JC,GT_new1,N,n);
                 %chsj_2=2;
                if  BDJC_new1==1&&BDJC_new2==1;
                    HZD(JCLS*2-1,:,DS+1)=GT_new1;%HZD表示所有個體
                    HZD(JCLS*2,:,DS+1)=GT_new2;
                    JCLS=JCLS+1;
                    %chesj_3=3;
                else
                    %chsj_33=333
                end;
            else
              % chsj_22=222
            end;
        else
            %chsj_11=1111
        end;
   end;
%以下操作為突變操作
	TBLS=1;%突變臨時變量
    while TBLS<=D(1)*Pm;
        TBXZ_new=ceil(rand*D(1));
        TBGT_new=CRE_TB(HZD(TBXZ_new,:,DS),D(2));
        QATB_new=SOL_QA(TBGT_new,V,A,P,N,n);
        TTB_new=SOV_T(TBGT_new,n,t1);
        BDVCTB_new=JUST_VC(QATB_new,TTB_new,TBGT_new,P,VC,N,n);
        if  BDVCTB_new==1;
            BDACTB_new=JUST_AC(QATB_new,AC,N,n);
            %chsj_tb1=1;
            if BDACTB_new==1;
                 BDJCTB_new=JUST_JC(QATB_new,JC,TBGT_new,N,n);
                % chsjtb_2=2;
                if  BDJCTB_new==1;
                    HZD(D(1)*Pc+TBLS,:,DS+1)=TBGT_new;
                    TBLS=TBLS+1;
                   % chesjtb_3=3;
                else
                    %chsjtb_33=333
                end;
            else
               %chsjtb_22=222
            end;
        else
           % chsjtb_11=1111
        end;
    end;
 %以下是精英主義
    JYXB_new=SOV_JYXB(FITD,D(1),Ps);
    for j=1:D(1)*Ps;
         HZD(D(1)*Pc+D(1)*Pm+j,:,DS+1)=HZD(JYXB_new(j),:,DS);
    end;
    DS=DS+1;
end;
%以下是求解最優(yōu)解
for i=1:D(1);
       QAD(:,:,i)=SOL_QA(HZD(i,:,DS),V,A,P,N,n);%求群體中的三軸加速度值
end;
FIT_ZZ=SOV_fitness(HZD(:,:,DS),QAD,KT,KJ,N,n,D(1));
ZYXB=SOV_ZYXB(FIT_ZZ,D(1));
H_ZY=HZD(ZYXB,:,DS)
T_HT=SOV_T(H_ZY,n,t1)
QA_HT=SOL_QA(H_ZY,V,A,P,N,n);
P_HT=SOV_P(P,QA_HT,H_ZY,V,N,n);