多智能体一致性编队控制仿真程序.rar

%the second-order systems 在二维空间上,三维空间编队控制 clear; A=[0 0 0 1;1 0 0 0;0 1 0 0;0 0 1 0];%邻接矩阵 L=[1 0 0 -1;-1 1 0 0;0 -1 1 0;0 0 -1 1];%拉普拉斯矩阵 h=0.005;%采样时间 % for i=1:1:4 % x(i)=i; % end x(1)=1; x(2)=0.2; x(3)=0; x(4)=5; % for i=1:1:4 % d(i)=i; % end dx(1)=0; dy(1)=0; dx(2)=1; dy(2)=0; dx(3)=1; dy(3)=1; dx(4)=0; dy(4)=1; y(1)=0; y(2)=1; y(3)=0.5; y(4)=2; vx(1)=0; vx(2)=0.5; vx(3)=0; vx(4)=0; vy(1)=0; vy(2)=0; vy(3)=0; vy(4)=0.1; z1(1,:)=[x(1) x(2) x(3) x(4)]; z2(1,:)=[y(1) y(2) y(3) y(4)]; m1(1,:)=[vx(1) vx(2) vx(3) vx(4)]; m2(1,:)=[vy(1) vy(2) vy(3) vy(4)]; c1=sqrt(x(1)^2+y(1)^2); c2=sqrt(x(2)^2+y(2)^2); c3=sqrt(x(3)^2+y(3)^2); c4=sqrt(x(4)^2+y(4)^2); z3(1,:)=[c1 c2 c3 c4]; cc1=sqrt(vx(1)^2+vy(1)^2); cc2=sqrt(vx(2)^2+vy(2)^2); cc3=sqrt(vx(3)^2+vy(3)^2); cc4=sqrt(vx(4)^2+vy(4)^2); z3(1,:)=[c1 c2 c3 c4]; m3(1,:)=[cc1 cc2 cc3 cc4]; % x1(0)=0;%第一个智能体的初始位置 % x2(0)=1;%第二个智能体的初始位置 % x3(0)=0.5;%第三个智能体的初始位置 % x4(0)=2;%第四个智能体的初始位置 b=1; g=30;%运行时间 for k=h:h:g r=round(k/h);%取整数 for i=1:1:4 u1(i)=A(i,1)*(x(1)-dx(1)-x(i)+dx(i))+A(i,2)*(x(2)-dx(2)-x(i)+dx(i))+A(i,3)*(x(3)-dx(3)-x(i)+dx(i))+A(i,4)*(x(4)-dx(4)-x(i)+dx(i))+b*A(i,1)*(vx(1)-vx(i))+b*A(i,2)*(vx(2)-vx(i))+b*A(i,3)*(vx(3)-vx(i))+b*A(i,4)*(vx(4)-vx(i)); u2(i)=A(i,1)*(y(1)-dy(1)-y(i)+dy(i))+A(i,2)*(y(2)-dy(2)-y(i)+dy(i))+A(i,3)*(y(3)-dy(3)-y(i)+dy(i))+A(i,4)*(y(4)-dy(4)-y(i)+dy(i))+b*A(i,1)*(vy(1)-vy(i))+b*A(i,2)*(vy(2)-vy(i))+b*A(i,3)*(vy(3)-vy(i))+b*A(i,4)*(vy(4)-vy(i)); vx(i)=vx(i)+u1(i)*h; vy(i)=vy(i)+u2(i)*h; m1(r+1,i)=vx(i); m2(r+1,i)=vy(i); x(i)=x(i)+vx(i)*h; y(i)=y(i)+vy(i)*h; z1(r+1,i)=x(i); z2(r+1,i)=y(i); z3(r+1,i)=sqrt(x(i)^2+y(i)^2); m3(r+1,i)=sqrt(vx(i)^2+vy(i)^2); end end t=0:h:g; plot(t,z1(:,1),'k',t,z1(:,2),'k',t,z1(:,3),'k',t,z1(:,4),'k') figure; plot(t,z2(:,1),'r',t,z2(:,2),'r',t,z2(:,3),'r',t,z2(:,4),'r') figure; %plot3(z1(:,1),z2(:,1),z3(:,1),'r-',z1(:,2),z2(:,2),z3(:,2),'c-',z1(:,3),z2(:,3),z3(:,3),'b-',z1(:,4),z2(:,4),z3(:,4),'g-') plot(t,m1(:,1),'k',t,m1(:,2),'k',t,m1(:,3),'k',t,m1(:,4),'k') hold on; plot(t,m2(:,1),'r',t,m2(:,2),'r',t,m2(:,3),'r',t,m2(:,4),'r') figure; % plot3(m1(:,1),m2(:,1),m3(:,1),'r-',m1(:,2),m2(:,2),m3(:,2),'c-',m1(:,3),m2(:,3),m3(:,3),'b-',m1(:,4),m2(:,4),m3(:,4),'g-') % % r1=round(15/h); % plot(vpa(z1(r1,1),2),vpa(z2(r1,1),2),'rs'); % hold on; % plot(vpa(z1(r1,2),2),vpa(z2(r1,2),2),'rs'); % hold on; % plot(z1(r1,3),z2(r1,3),'rs'); % hold on; % plot(z1(r1,4),z2(r1,4),'rs'); % figure; D1(:,1)=roundn(z1(:,1),-2); D1(:,2)=roundn(z1(:,2),-2); D1(:,3)=roundn(z1(:,3),-2); D1(:,4)=roundn(z1(:,4),-2); D2(:,1)=roundn(z2(:,1),-2); D2(:,2)=roundn(z2(:,2),-2); D2(:,3)=roundn(z2(:,3),-2); D2(:,4)=roundn(z2(:,4),-2); plot(D1(1,1),D2(1,1),'rs',D1(1,2),D2(1,2),'rs',D1(1,3),D2(1,3),'rs',D1(1,4),D2(1,4),'rs');%初始的位置 figure; r2=round(5/h); plot(D1(r2,1),D2(r2,1),'rs',D1(r2,2),D2(r2,2),'rs',D1(r2,3),D2(r2,3),'rs',D1(r2,4),D2(r2,4),'rs'); figure; r2=round(10/h); plot(D1(r2,1),D2(r2,1),'rs',D1(r2,2),D2(r2,2),'rs',D1(r2,3),D2(r2,3),'rs',D1(r2,4),D2(r2,4),'rs'); figure; r2=round(15/h); plot(D1(r2,1),D2(r2,1),'rs',D1(r2,2),D2(r2,2),'rs',D1(r2,3),D2(r2,3),'rs',D1(r2,4),D2(r2,4),'rs'); figure; r2=round(20/h); plot(D1(r2,1),D2(r2,1),'rs',D1(r2,2),D2(r2,2),'rs',D1(r2,3),D2(r2,3),'rs',D1(r2,4),D2(r2,4),'rs'); % figure; % r2=round(25/h); % plot(D1(r2,1),D2(r2,1),'rs',D1(r2,2),D2(r2,2),'rs',D1(r2,3),D2(r2,3),'rs',D1(r2,4),D2(r2,4),'rs'); % figure; % r2=round(30/h);%30s时候位置 % plot(D1(r2,1),D2(r2,1),'rs',D1(r2,2),D2(r2,2),'rs',D1(r2,3),D2(r2,3),'rs',D1(r2,4),D2(r2,4),'rs'); % hold on; % plot(z1(r2,3),z2(r2,3),'rs'); % hold on; % plot(z1(r2,4),z2(r2,4),'rs'); % scatter3(m1(r1,1),m2(r1,1),m3(r1,1),'p',m1(r1,2),m2(r1,2),m3(r1,2),'p',m1(r1,3),m2(r1,3),m3(r1,3),'p',m1(r1,4),m2(r1,4),m3(r1,4),'p') % plot(z1(:,1),z2(:,1),'r'); % hold on; % plot(z1(:,2),z2(:,2),'k'); % hold on; % plot(z1(:,3),z2(:,3),'b'); % hold on; % plot(z1(:,4),z2(:,4),'y'); % % plot(z1(1,:),z2(:,1),'r')