tiao1.m

从事多机器人探测控制的朋友可以参考我写的源程序

% clc
% clear
%----------------------设立初始环境：20*50，目标数为40，可移动障碍物数为100，不可移动障碍物数为80；其中0表示空地，1表示可移动障碍物，2表示不可移动障碍物,3表示
%目标3，4表示墙.
%---------------------------------------------机器人能看见感应范围内的任何物体。

% tic
% wangge=zeros(20,50);robotnum=3;
% for i=1:20
%     wangge(i,1)=4;wangge(i,50)=4;
% end
% for j=1:50
%     wangge(1,j)=4;wangge(20,j)=4;
% end
% ii=1;
% for i=1:1000
%     j=round(rand(1)*19)+1;
%     k=round(rand(1)*49)+1;
%     if j~=1&j~=20&k~=1&k~=50&wangge(j,k)==0
%        wangge(j,k)=1;ii=ii+1;
%     end
%     if ii==101
%         break
%     end
% end
% ii=1;
% for i=1:1000
%     j=round(rand(1)*19)+1;
%     k=round(rand(1)*49)+1;
%     if j~=1&j~=20&k~=1&k~=50&wangge(j,k)==0
%        wangge(j,k)=2;ii=ii+1;
%     end
%     if ii==41
%         break
%     end
% end
% ii=1;
% for i=1:1000
%     j=round(rand(1)*19)+1;
%     k=round(rand(1)*49)+1;
%     if j~=1&j~=20&k~=1&k~=50&wangge(j,k)==0
%        wangge(j,k)=3;ii=ii+1;
%     end
%     if ii==81
%         break
%     end
% end
%---------------------------------------------
%---------------------------------------------


ppp=wangge;lujing=[5 14 23];bianjie=27;lovelist=zeros(20,50)-1;qiangbianjie=0;
%---------------------------------------------lovelist为探测状态矩阵，-1表示没有探测，0表示已
%经探测了的网格，1表示存在可移动障碍物，2为不可移动障碍物，3目标，4墙，50+为机器人。
%---------------------------------------------机器人初始位置robotwg:3*2;ppp为机器人探测时的环境状态：20*50。
% for i=2:50
%     if ppp(2,i)==0
%        robotwg(1,:)=[2,i];ppp(2,i)=51;lovelist(2,i)=51;
%        break;
%     end
% end
% for i=2:robotnum
%    for j=(robotwg(i-1,2)+1):50
%        if ppp(2,j)==0
%            robotwg(i,:)=[2,j];ppp(2,j)=50+i;lovelist(2,j)=51;break;
%        end
%    end
% end
% ---------------------------------------------
% ---------------------------------------------


robotwg=[2,2;2,4;2,6];
lovelist(2,2)=51;lovelist(2,4)=52;lovelist(2,6)=53;
ppp(2,2)=51;ppp(2,4)=52;ppp(2,6)=53;


runi=1;
for i=1:robotnum guiji(runi,2*i-1:2*i)=[robotwg(i,1),robotwg(i,2)];end % ---------------------------------------------机器人运动的轨迹。


%---------------------------------------------机器人运动控制部分。
% ---------------------------------------------主体部分
for runi=1:150      
        M1=robotwg;
        [runt,jjkk]=runt(guiji,robotnum,runi);
        zwd=zhouweidian(M1,robotnum);
        [M,lovelist]=M(zwd,ppp,lovelist,robotnum,robotwg);%---------------探测。
%         [scanp,ffj]=scanp(lovelist,robotwg,lujing,robotnum);%---------------扫描。
        [bjval,aaj]=bjval(lovelist,zwd,robotnum);
        [goatr,bbj]=goatr(robotnum,M);
        [rexp,ccj]=rexp(lovelist,robotnum,zwd);
%         [team,ddj]=team(lujing,zwd,robotnum,scanp);
        [cond,eej]=cond(M,robotnum,zwd);
        for i=1:robotnum
            for j=1:8
                if cond(i,j)==0
                    val(i,j)=0;
%                 elseif 
                else
                val(i,j)=bjval(i,j)*50+goatr(i,j)*300+rexp(i,j)*1.2+cond(i,j)*15+runt(zwd(i,j*2-1),zwd(i,j*2))*(-30);%------------------产生适值。
                end
            end
        end

  % ---------------------------------------------挑选运动方向。没有进行一般化处理，当robotnum改变时，此处得改。

        maxval=0.001;i=1;
        for vvi=1:8
               va=val(1,vvi);an=zwd(1,2*vvi-1);am=zwd(1,2*vvi);
               for vvj=1:8
                       vb=val(2,vvj);bn=zwd(2,2*vvj-1);bm=zwd(2,2*vvj);
                       for vvk=1:8
                               vc=val(3,vvk);cn=zwd(3,vvk*2-1);cm=zwd(3,vvk*2);
                               if [an,am]==[bn,bm]|[an,am]==[cn,cm]|[bn,bm]==[cn,cm]
                               else   bb=va+vb+vc;
                                      if bb>maxval
                                         maxval=bb;pt=[an am;bn bm;cn ,cm];
                                      end
                               end
                       end
               end
         end
   % ---------------------------------------------机器人产生运动。
        for i=1:robotnum
            if lovelist(pt(i,1),pt(i,2))==3
                lovelist(pt(i,1),pt(i,2))=50+i;lovelist(robotwg(i,1),robotwg(i,2))=0;
                ppp(pt(i,1),pt(i,2))=50+i;ppp(robotwg(i,1),robotwg(i,2))=0;robotwg(i,1)=pt(i,1);robotwg(i,2)=pt(i,2);
            elseif lovelist(pt(i,1),pt(i,2))==1
                lovelist(pt(i,1),pt(i,2))=50+i;lovelist(robotwg(i,1),robotwg(i,2))=1;
                ppp(pt(i,1),pt(i,2))=50+i;ppp(robotwg(i,1),robotwg(i,2))=1;robotwg(i,1)=pt(i,1);robotwg(i,2)=pt(i,2);
            else lovelist(pt(i,1),pt(i,2))=50+i;lovelist(robotwg(i,1),robotwg(i,2))=0;
                ppp(pt(i,1),pt(i,2))=50+i;ppp(robotwg(i,1),robotwg(i,2))=0;robotwg(i,1)=pt(i,1);robotwg(i,2)=pt(i,2);
            end
        end
        for i=1:robotnum guiji(runi+1,2*i-1:2*i)=[robotwg(i,1),robotwg(i,2)];end %----------轨迹.
% ---------------------------------------------
% ---------------------------------------------


% ---------------------------------------------判断本区域是否已经探测完成.
lujb=[0 0 0];cc=0;
for i=1:20
    for j=1:50
        if lovelist(i,j)==-1|lovelist(i,j)==3
            cc=cc+1;break
        end
    end
    if cc>0
        break
    end
end
% for i=1:robotnum
%     for j=(-4+lujing(1,i)):(4+lujing(1,i))
%             if 1<=j&j<=50
%                 for k=1:20
%                     if lovelist(k,j)<0|lovelist(k,j)==3
%                         lujb(i)=1;break
%                     end
%                 end
%                 if lujb(i)==1;
%                     break
%                 end
%             end
%     end
%                 if lujb(i)==1
%                 elseif bianjie+9<50%------------------机器人未到达最后一个区域。
%                        lujing(i)=bianjie+5;bianjie=bianjie+9;
%                 elseif bianjie<50%------------------机器人首次到达最后一个区域。
%                        lujing(i)=bianjie+round((50-bianjie)/2);bianjie=50;
%                 elseif max(lujing)<50&lujing(i)<max(lujing)%------------------其它机器人到达最后一个未完全访问区域。
%                        lujing(i)=max(lujing);
%                 elseif min(lujing)==max(lujing)
%                        lujing(i)=50;cc=1;%------------------3个机器人都探测完最后一个区域。
%                 end
% end
% ---------------------------------------------
% ---------------------------------------------

if cc==0
    break;
end
end
            
% save data
                
%          
%         for i=1:robotnum
%             if runo(i)==0
%                 maxlj=max(lujing);
%         



for k=1:20
     for j=1:50
     if lovelist(k,j)==0
         plot(k,j,'s');hold on
     elseif lovelist(k,j)==-1
         plot(k,j,'m*');hold on
     elseif lovelist(k,j)==1
         plot(k,j,'p');hold on
     elseif lovelist(k,j)==2
         plot(k,j,'<');hold on
     elseif lovelist(k,j)==3
         plot(k,j,'m>');hold on
     elseif lovelist(k,j)==4
         plot(k,j,'v');hold on
     elseif lovelist(k,j)==51
         plot(k,j,'r-x');hold on   
     elseif lovelist(k,j)==52
         plot(k,j,'go');hold on
     elseif lovelist(k,j)==53
         plot(k,j,'bD');hold on   
     end
     end
 end
 
     for  i=1:(runi+1)
     cn=guiji(i,1); cm=guiji(i,2);cnn=guiji(i,3); cmm=guiji(i,4);cnnn=guiji(i,5);cmmm=guiji(i,6);
     plot(cn,cm,'rx');hold on
     plot(cnn,cmm,'go');hold on
     plot(cnnn,cmmm,'bD');hold on
     end
     for j=2:(runi+1)
         cn=guiji(j-1,1); cm=guiji(j-1,2);cnn=guiji(j,1); cmm=guiji(j,2);
         bn=guiji(j-1,3); bm=guiji(j-1,4);bnn=guiji(j,3); bmm=guiji(j,4);
         an=guiji(j-1,5); am=guiji(j-1,6);ann=guiji(j,5); amm=guiji(j,6);
         for k=1:50
             a=cn+(cnn-cn)*k/50;b=cm+(cmm-cm)*k/50;
             plot(a,b,'r-');hold on
             aa=bn+(bnn-bn)*k/50;bb=bm+(bmm-bm)*k/50;
             plot(aa,bb,'g-');hold on
             aaa=an+(ann-an)*k/50;bbb=am+(amm-am)*k/50;
             plot(aaa,bbb,'b-');hold on
         end
     end


% toc