?? oprt_zh.asv
字號:
clear
D=[50,5,1000];%依次表示行,列,頁
N=3;n=6;%依次表示軸數和插值點數
%以P表示位置,V表示速度,A表示加速度,J表示加加速度
%T表示時間,h表示時間間隔
VC=[100,95,100];%依次設置第一軸,第二軸,第三軸的速度約束
AC=[70,75,75];%依次設置第一軸,第二軸,第三軸的加速度約束
JC=[70,85,70];%依次設置第一軸,第二軸,第三軸的沖擊約束
V=zeros(N,n);%設置速度邊界
A=zeros(N,n);%設置加速度邊界
KT=1;KJ=0.2;
t1=0;%起始時間
Ps=0.04;Pm=0.16;Pc=0.8;
P(1,1)=120;P(1,3)=90;P(1,4)=45;P(1,6)=0;
P(2,1)=-10;P(2,3)=60;P(2,4)=40;P(2,6)=100;
P(3,1)=0;P(3,3)=-20;P(3,4)=50;P(3,6)=70;
%P(3,1)=0;P(3,3)=-20;P(3,4)=50;P(3,6)=70;%以上三行設置初始位置點
%群體數據的初始化
%rand('state',sum(50*clock));
rand('state',1);
JUST=1;
while JUST<=D(1)
%確定時間間隔及時間表
for i=1:n-1;
H(i)=0.5+rand*2.5;
end;%h(i)表示各插值點的時間間隔
T_ini=SOV_T(H,n,t1);
%T(i)各插值點的即時時間
%判斷點是否生成
QA=SOL_QA(H,V,A,P,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;
%以下是求解最優解
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);
HTBL=3;%可選擇1,2,3表示第一,二,三軸
for HTBL=1:3;
PHTL=P_HT(HTBL,:);%畫圖的位置
QAHTL=QA_HT(HTBL,:);%畫圖用的加速度
VL=V(HTBL,:);%zeros(1,n);%畫圖用的速度
HL=H_ZY;%畫圖用的時間間隔
TL=T_HT;%畫圖用的插值點時間
[QJ_MAX,QJ_ZH]=SOV_QJ(QA_HT,HL,N,n)%求出最大的沖擊
T1=TL(1):0.01:TL(2);
T2=TL(2):0.01:TL(3);
T3=TL(3):0.01:TL(4);
T4=TL(4):0.01:TL(5);
T5=TL(5):0.01:TL(6);
Q1=QAHTL(1)/(6*HL(1))*(TL(2)-T1).^3+QAHTL(2)/(6*HL(1))*(T1-TL(1)).^3+...
(PHTL(2)/HL(1)-HL(1)*QAHTL(2)/6)*(T1-TL(1))+(PHTL(1)/HL(1)-HL(1)*QAHTL(1)/6)*(TL(2)-T1);
Q2=QAHTL(2)/(6*HL(2))*(TL(3)-T2).^3+QAHTL(3)/(6*HL(2))*(T2-TL(2)).^3+...
(PHTL(3)/HL(2)-HL(2)*QAHTL(3)/6)*(T2-TL(2))+(PHTL(2)/HL(2)-HL(2)*QAHTL(2)/6)*(TL(3)-T2);
Q3=QAHTL(3)/(6*HL(3))*(TL(4)-T3).^3+QAHTL(4)/(6*HL(3))*(T3-TL(3)).^3+...
(PHTL(4)/HL(3)-HL(3)*QAHTL(4)/6)*(T3-TL(3))+(PHTL(3)/HL(3)-HL(3)*QAHTL(3)/6)*(TL(4)-T3);
Q4=QAHTL(4)/(6*HL(4))*(TL(5)-T4).^3+QAHTL(5)/(6*HL(4))*(T4-TL(4)).^3+...
(PHTL(5)/HL(4)-HL(4)*QAHTL(5)/6)*(T4-TL(4))+(PHTL(4)/HL(4)-HL(4)*QAHTL(4)/6)*(TL(5)-T4);
Q5=QAHTL(5)/(6*HL(5))*(TL(6)-T5).^3+QAHTL(6)/(6*HL(5))*(T5-TL(5)).^3+...
(PHTL(6)/HL(5)-HL(5)*QAHTL(6)/6)*(T5-TL(5))+(PHTL(5)/HL(5)-HL(5)*QAHTL(5)/6)*(TL(6)-T5);
V1=-QAHTL(1)/(2*HL(1))*(TL(2)-T1).^2+QAHTL(2)/(2*HL(1))*(T1-TL(1)).^2+...
(PHTL(2)/HL(1)-HL(1)*QAHTL(2)/6)-(PHTL(1)/HL(1)-HL(1)*QAHTL(1)/6);
V2=-QAHTL(2)/(2*HL(2))*(TL(3)-T2).^2+QAHTL(3)/(2*HL(2))*(T2-TL(2)).^2+...
(PHTL(3)/HL(2)-HL(2)*QAHTL(3)/6)-(PHTL(2)/HL(2)-HL(2)*QAHTL(2)/6);
V3=-QAHTL(3)/(2*HL(3))*(TL(4)-T3).^2+QAHTL(4)/(2*HL(3))*(T3-TL(3)).^2+...
(PHTL(4)/HL(3)-HL(3)*QAHTL(4)/6)-(PHTL(3)/HL(3)-HL(3)*QAHTL(3)/6);
V4=-QAHTL(4)/(2*HL(4))*(TL(5)-T4).^2+QAHTL(5)/(2*HL(4))*(T4-TL(4)).^2+...
(PHTL(5)/HL(4)-HL(4)*QAHTL(5)/6)-(PHTL(4)/HL(4)-HL(4)*QAHTL(4)/6);
V5=-QAHTL(5)/(2*HL(5))*(TL(6)-T5).^2+QAHTL(6)/(2*HL(5))*(T5-TL(5)).^2+...
(PHTL(6)/HL(5)-HL(5)*QAHTL(6)/6)-(PHTL(5)/HL(5)-HL(5)*QAHTL(5)/6);
A1=QAHTL(1)/HL(1)*(TL(2)-T1)+QAHTL(2)/HL(1)*(T1-TL(1));
A2=QAHTL(2)/HL(2)*(TL(3)-T2)+QAHTL(3)/HL(2)*(T2-TL(2));
A3=QAHTL(3)/HL(3)*(TL(4)-T3)+QAHTL(4)/HL(3)*(T3-TL(3));
A4=QAHTL(4)/HL(4)*(TL(5)-T4)+QAHTL(5)/HL(4)*(T4-TL(4));
A5=QAHTL(5)/HL(5)*(TL(6)-T5)+QAHTL(6)/HL(5)*(T5-TL(5));
J1=(T1*(QAHTL(2)-QAHTL(1))/HL(1))./(T1+eps);
J2=T2*(QAHTL(3)-QAHTL(2))/HL(2)./T2;
J3=T3*(QAHTL(4)-QAHTL(3))/HL(3)./T3;
J4=T4*(QAHTL(5)-QAHTL(4))/HL(4)./T4;
J5=T5*(QAHTL(6)-QAHTL(5))/HL(4)./T5;
subplot(3,3,HTBL)
plot(T1,Q1,'k-',T2,Q2,'k:',T3,Q3,'k-.',T4,Q4,'k--',T5,Q5,'k-'),legend('位移','速度','加速度','沖擊')
hold on
plot(T1,Q1,'k-',T2,Q2,'k-',T3,Q3,'k-',T4,Q4,'k-',T5,Q5,'k-')
plot(T1,V1,'k:',T2,V2,'k:',T3,V3,'k:',T4,V4,'k:',T5,V5,'k:')
plot(T1,A1,'k-.',T2,A2,'k-.',T3,A3,'k-.',T4,A4,'k-.',T5,A5,'k-.')
plot(T1,J1,'k--',T2,J2,'k--',T3,J3,'k--',T4,J4,'k--',T5,J5,'k--')
xlabel('t/s');
ylabel('位移,速度,加速度,沖擊');
hold off
end;
%title('第三關節的位置、速度、加速度、沖擊曲線')
%grid
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -