多智能体系统的仿真_matlab源码.zip

clear all; close all; clc; c1=0;b1=0;b2=0;b3=0; c2=0; c3=0; x11=1; x12=1; x13=5; v1=0; w1=0; x21=1.1; x22=1.1; x23=6; v2=0; w2=0; x31=1.2; x32=1.2; x33=7; v3=0; w3=0; L1=2; k1=34; k2=2.7; k3=1.3; k4=1.5;k5=1.1; p=9/7; dt=0.001; T=20; for i=1:T/dt t=i*dt; x11=(v1+1*sin(t))*cos(x13)*dt+x11; x12=(v1+1*sin(t))*sin(x13)*dt+x12; x13=w1*dt+x13; y11=-x13; y12=-x11*sin(x13)+x12*cos(x13); y13=x11*cos(x13)+x12*sin(x13); x21=(v2+1*sin(t))*cos(x23)*dt+x21; x22=(v2+1*sin(t))*sin(x23)*dt+x22; x23=w2*dt+x23; y21=-x23; y22=-x21*sin(x23)+x22*cos(x23); y23=x21*cos(x23)+x22*sin(x23); x31=(v3+1*sin(t))*cos(x33)*dt+x31; x32=(v3+1*sin(t))*sin(x33)*dt+x32; x33=w3*dt+x33; y31=-x33; y32=-x31*sin(x33)+x32*cos(x33); y33=x31*cos(x33)+x32*sin(x33); if t<10 c1=k1*nthroot((nthroot(y13^9,7)+k2^p*0.05*(y12-y22))^5,9)*dt+c1; s1=y13+c1; b1=k5*L1*sign(s1)*dt+b1; v12=-k4*L1^(1/2)*sign(s1)*(abs(s1))^(1/2)-b1; u11=1; u12=-k1*nthroot((nthroot(y13^9,7)+k2^p*0.05*(y12-y22))^5,9)+v12; c2=k1*nthroot((nthroot(y23^9,7)+k2^p*0.05*((y22-y12)+(y22-y32)))^5,9)*dt+c2; s2=y23+c2; b2=k5*L1*sign(s2)*dt+b2; v22=-k4*L1^(1/2)*sign(s2)*(abs(s2))^(1/2)-b2; u21=1; u22=-k1*nthroot((nthroot(y23^9,7)+k2^p*0.05*((y22-y12)+(y22-y32)))^5,9)+v22; c3=k1*nthroot((nthroot(y33^9,7)+k2^p*0.05*(y32-y22))^5,9)*dt+c3; s3=y33+c3; b3=k5*L1*sign(s3)*dt+b3; v32=-k4*L1^(1/2)*sign(s3)*(abs(s3))^(1/2)-b3; u31=1; u32=-k1*nthroot((nthroot(y33^9,7)+k2^p*0.05*(y32-y22))^5,9)+v32; else u11=-k2*nthroot((y11-y21)^7,9); c1=k1*nthroot((nthroot(y13^9,7)+k2^p*0.05*(y12-y22))^5,9)*dt+c1; s1=y13+c1; b1=k5*L1*sign(s1)*dt+b1; v12=-k4*L1^(1/2)*sign(s1)*(abs(s1))^(1/2)-b1; u12=-k1*nthroot((nthroot(y13^9,7)+k2^p*0.05*(y12-y22))^5,9)+v12; u21=-k2*nthroot(((y21-y11)+(y21-y31))^7,9); c2=k1*nthroot((nthroot(y23^9,7)+k2^p*0.05*((y22-y12)+(y22-y32)))^5,9)*dt+c2; s2=y23+c2; b2=k5*L1*sign(s2)*dt+b2; v22=-k4*L1^(1/2)*sign(s2)*(abs(s2))^(1/2)-b2; u22=-k1*nthroot((nthroot(y23^9,7)+k2^p*0.05*((y22-y12)+(y22-y32)))^5,9)+v22; u31=-k2*nthroot((y31-y21)^7,9); c3=k1*nthroot((nthroot(y33^9,7)+k2^p*0.05*(y32-y22))^5,9)*dt+c3; s3=y33+c3; b3=k5*L1*sign(s3)*dt+b3; v32=-k4*L1^(1/2)*sign(s3)*(abs(s3))^(1/2)-b3; u32=-k1*nthroot((nthroot(y33^9,7)+k2^p*0.05*(y32-y22))^5,9)+v32; end v1=(-x11*sin(x13)+x12*cos(x13))*u11+u12; w1=-u11; v2=(-x21*sin(x23)+x22*cos(x23))*u21+u22; w2=-u21; v3=(-x31*sin(x33)+x32*cos(x33))*u31+u32; w3=-u31; x11p(i)=x11; x12p(i)=x12; x13p(i)=x13; x21p(i)=x21; x22p(i)=x22; x23p(i)=x23; x31p(i)=x31; x32p(i)=x32; x33p(i)=x33; v1p(i)=v1; w1p(i)=w1; v2p(i)=v2; w2p(i)=w2; v3p(i)=v3; w3p(i)=w3; end t=dt:dt:T; figure; plot(t,x11p,t,x21p,t,x31p) legend( 'x_1','x_2','x_3'); xlabel('time (sec)'); figure; plot(t,x12p,t,x22p,t,x32p) legend( 'y_1','y_2','y_3'); xlabel('time (sec)'); figure; plot(t,x13p,t,x23p,t,x33p) legend( '\theta_1','\theta_2','\theta_3'); xlabel('time (sec)'); figure; plot(t,v1p,t,w1p) legend( 'v_1','w_1'); axis([0 20 -5 5]); xlabel('time (sec)'); figure; plot(t,v2p,t,w2p) legend( 'v_2','w_2'); axis([0 20 -5 5]); xlabel('time (sec)'); figure; plot(t,v3p,t,w3p) legend( 'v_3','w_3'); axis([0 20 -5 5]); xlabel('time (sec)');