一阶系统编队.zip_一致性 编队_一致性仿真_多智能体系统_多智能体编队_智能体

c=[-1 1 0 0 0 0 -1 1 0 0 0 0 -1 1 0 0 0 0 -1 1 0 0 0 0 0 ]; %交互矩阵 theta0=[30,-40,70,120,60]; %初始方向角 position0={[30 5] [20 6] [0 0] [3 40] [20 20]}; speed=5; distance=7; t=0:0.01:20; %时间离散化 theta(1,:)=theta0; %传递初始方向角到方向角变量里面 for i=2:length(t) theta(i,:)=expm(t(i)*c)*theta0'; end a=c; for i=1:length(position0) a(i,i)=0; %将交互矩阵转化成a矩阵 end for j=1:length(position0) %每个点的位置存于一列向量中 position{1,j}(1,1)=position0{1,j}(1,1); %位置初始值 position{1,j}(1,2)=position0{1,j}(1,2); %位置初始值 end for i=2:length(theta) %第j个点运动的轨迹点 for j=1:length(position0) vx=0;vy=0;n=0.1; for k=1:length(position0) kx=(position{i-1,j}(1,1)-position{i-1,k}(1,1))/(abs(position{i-1,j}(1,1)-position{i-1,k}(1,1))+n); ky=(position{i-1,j}(1,2)-position{i-1,k}(1,2))/(abs(position{i-1,j}(1,2)-position{i-1,k}(1,2))+n); vx=vx-a(j,k)*kx*(abs(position{i-1,j}(1,1)-position{i-1,k}(1,1))-distance); vy=vy-a(j,k)*ky*(abs(position{i-1,j}(1,2)-position{i-1,k}(1,2))-distance); end position{i,j}(1,1)=position{i-1,j}(1,1)+0.01*(speed*cosd(theta(i-1,j))+vx); %新点在x上增加后的值 position{i,j}(1,2)=position{i-1,j}(1,2)+0.01*(speed*sind(theta(i-1,j))+vy); %新点在y上增加后的值 end end for j=1:length(position0) for i=1:length(theta) xPosition(i,j)=position{i,j}(1,1); %横坐标矩阵 yPosition(i,j)=position{i,j}(1,2); %纵坐标矩阵 end end for i=1:50:length(theta) %延时画图 plot(xPosition(i,1),yPosition(i,1),'-r*',xPosition(i,2),yPosition(i,2),'-g+',xPosition(i,3),yPosition(i,3),'-bo',xPosition(i,4),yPosition(i,4),'-cs',xPosition(i,5),yPosition(i,5),'-kv'); hold on pause(0.1) end hold off plot(xPosition(1:50:length(theta),1),yPosition(1:50:length(theta),1),'-r*',xPosition(1:50:length(theta),2),yPosition(1:50:length(theta),2),'-g+',xPosition(1:50:length(theta),3),yPosition(1:50:length(theta),3),'-bo',xPosition(1:50:length(theta),4),yPosition(1:50:length(theta),4),'-cs',xPosition(1:50:length(theta),5),yPosition(1:50:length(theta),5),'-kv'); %画图 legend('robot 1','robot 2','robot 3','robot 4','robot 5') xlabel('x(m)','FontSize',12); ylabel('y(m)','FontSize',12); %set(gca,'xTick',0:10:60);