一阶多智能体matlab,我想实现MATLAB多智能体仿真，不知道哪里出问题了，希望........

想实现三个小车从无序到三角形编队前进&＃xff0c;可是仿真出来是这样的

%% 小车的状态重新定义

clear;

clf;

clc;

n &＃61; 3;

l &＃61; 0.05;

k &＃61; 0.02;

% c &＃61; [0.2;0];

c &＃61;2;

T &＃61; 0.1;

loop &＃61; 200;

co(1,1) &＃61; &＃39;r&＃39;;

co(1,2) &＃61; &＃39;b&＃39;;

co(1,3) &＃61; &＃39;g&＃39;;             %颜色

a &＃61; zeros(2,n);

theta &＃61; zeros(1,n);

a &＃61;[0.57,0.45,0.74;-0.51,-0.22,-0.31];

theta &＃61; [0.43,0.60,-0.80];

p &＃61; zeros(1,n);

q &＃61; zeros(1,n);

u &＃61; zeros(2,n);

invA &＃61; zeros(2,2);

difgammaTrans &＃61; zeros(2,n);

A &＃61; zeros(2,2);

% sk &＃61; zeros(loop,n);

%%

a(1,:) &＃61; a(1,:) &＃43; l*cos(theta(1:n));

a(2,:) &＃61; a(2,:) &＃43; l*sin(theta(1:n));

for f &＃61; 1:loop

for i &＃61; 1:n

if i ~&＃61; n

%             diffgammaTrans(:,i) &＃61; 2*(abs(a(:,i)-a(:,i&＃43;1)-c));

%             difgammaTrans(:,i) &＃61; 2*[a(1,i)-a(1,i&＃43;1);a(2,i)-a(2,i&＃43;1)-c];

difgammaTrans(:,i) &＃61; 2*[a(1,i)-a(1,i&＃43;1)-c;a(2,i)-a(2,i&＃43;1)];

else

%             diffgammaTrans(:,i) &＃61; 2*(abs(a(:,i)-a(:,1)-(n-1)*c));

%             difgammaTrans(:,n) &＃61; 2*[a(1,n)-a(1,1);a(2,n)-a(2,1)-(n-1)*c];

difgammaTrans(:,n) &＃61; 2*[a(1,n)-a(1,1)&＃43;(n-1)*c;a(2,n)-a(2,1)];

end

invA &＃61; (1/l)*[l*cos(theta(i)),l*sin(theta(i));-sin(theta(i)),cos(theta(i))];

A &＃61; [cos(theta(i)),-l*sin(theta(i));sin(theta(i)),l*cos(theta(i))];

u(:,i) &＃61; -1/(2*T)*k*(invA)*(difgammaTrans(:,i));

%        a(:,i) &＃61; a(:,i) &＃43; T*A*u(:,i);                %% 位置更新

zj(:,i) &＃61; a(:,i);

a(1,i) &＃61; a(1,i) &＃43; T*A(1,:)*u(:,i);

a(2,i) &＃61; a(2,i) &＃43; T*A(2,:)*u(:,i);

sk(f,i) &＃61; sqrt((zj(1,i)-a(1,i))^2&＃43;(zj(2,i)-a(2,i))^2);

theta(i) &＃61; theta(i)&＃43;T*u(2,i);

subplot(2,1,1);

hold on

%         plot(p(1,i),q(1,i),&＃39;o&＃39;);

plot(a(1,i),a(2,i),&＃39;o&＃39;,&＃39;color&＃39;,co(1,i));

hold off

title([&＃39;Iteration: &＃39; dec2base(f,10)]);

subplot(2,1,2);

hold on

plot(f,sk(f,i),&＃39;*&＃39;,&＃39;color&＃39;,co(i));

hold off

getframe;                                    %% 作图

end

%% 此段为实现小车被移动后重新编队效果&＃xff0c;需要修改loop值

%     if f&＃61;&＃61;200

%         a(1,1)&＃61;2;

%         a(2,1)&＃61;0.2;

%         theta(1)&＃61;0.4;

%     end

%     if f&＃61;&＃61;350

%         a(1,2)&＃61;0;

%         a(2,2)&＃61;0.1;

%         theta(2)&＃61;0.5;

%     end

end

legend(&＃39;robot1&＃39;,&＃39;robot2&＃39;,&＃39;robot3&＃39;);

% legend(&＃39;robot1&＃39;,&＃39;robot2&＃39;,&＃39;robot3&＃39;);

% figure(2);

% for ti &＃61; 1:loop

%     for ii &＃61; 1:n

%         hold on

%     plot(ti,sk(ti,ii),co(ii));

%     hold off

%     end

% end

% % xlebal(&＃39;t(s)&＃39;);

% % xlebal(&＃39;Error&＃39;);

% %标注出错&＃xff0c;原因查找

% legend(&＃39;robot1&＃39;,&＃39;robot2&＃39;,&＃39;robot3&＃39;);

%% 最后的关键在于节点之间的理想状态之间的关系没有描述好&＃xff0c;i和i&＃43;1之间的c描述为&＃43;2&＃xff0c;而n和1之间则需要-2。

希望大家可以指导一下我该怎么修改&＃xff0c;谢谢啦

补充内容 (2018-6-1 19:38):

是的撒多撒

2018-4-28 13:05 上传