tprob.m

机器人协同控制流函数matlab程序实现

         end 
     end  
 end
 
 %-------------------------------------------------------------------------
 %-------------------------------------------------------------------------
 %-------------------------------------------------------------------------
 %------------


%-------------------------------------------------------------------------------------------------------如果有右区域，进行判断   
if length(R)~=0
    num_LR=0;%和leader相连的左区间agent数
    for i=1:length(R(1,:)) 
        if R(2,i)~=0
        num_LR=num_LR+1;
        break
        end
    end
%%--------------------------------------------------------leader和左区域agent相连的情况,确定l
if(num_LR~=0)
r1id=1;
 for i=2:Rob_num;
    if Dis_Mat(1,i)~=0
          if x(i,2)<yl  %判断leader的左子树
            R1(1,r1id)=i
            R1(2,r1id)=Dis_Mat(1,i);
            r1id=r1id+1;
          end
      end
  end
%确定leader的左节点
    rtemp=min(R1(2,:));
    for j=1:length(R1(1,:))%最小值对应的agent的id
        if rtemp==R1(2,j)
            r=R1(1,j);
            Probability_Matrix(R1(1,j),1)=1;%找到了与leader相连的左节点
            break
        end
    end
%%---------------------------------------------------------------leader不和左区域agent相连的情况,确定l
else
    Temp=[];
    k=1;
    for i=1:length(R(1,:))
        Temp(1,k)=R(1,i);
        Temp(2,k)=x(R(1,i),1);
        k=k+1;
    end
    rtemp=max(Temp(2,:));
    for j=1:length(Temp(1,:))
        if rtemp==Temp(2,j)
            r=Temp(1,j);
            Probability_Matrix(r,1)=0;
            break
        end
    end
end
end
%------------------------------------R区域-----------------------------------
if length(R)~=0
    [Probability_Matrix,R11,r11,R12,r12]=Choice(R,r,Probability_Matrix,Dis_Mat,x);
     if length(R11)~=0             
         [Probability_Matrix,R21,r21,R22,r22]=Choice(R11,r11,Probability_Matrix,Dis_Mat,x)
         if length(R21)~=0       
             [Probability_Matrix,R31,r31,R32,r32]=Choice(R21,r21,Probability_Matrix,Dis_Mat,x)
             if length(R31)~=0   
                 [Probability_Matrix,R41,r41,R42,r42]=Choice(R31,r31,Probability_Matrix,Dis_Mat,x)
                 if length(R41)~=0  
                     [Probability_Matrix,R51,r51,R52,r52]=Choice(R41,r41,Probability_Matrix,Dis_Mat,x)
                     if length(R51)~=0   
                         [Probability_Matrix,R61,r61,R62,r62]=Choice(R51,r51,Probability_Matrix,Dis_Mat,x)
                         if length(R61)~=0 
                             [Probability_Matrix,R71,r71,R72,r72]=Choice(R61,r61,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R62)~=0   
                             [Probability_Matrix,R73,r73,R74,r74]=Choice(R62,r62,Probability_Matrix,Dis_Mat,x)
                         end    
                     end
                     if length(R52)~=0   
                         [Probability_Matrix,R63,r63,R64,r64]=Choice(R52,r52,Probability_Matrix,Dis_Mat,x)
                         if length(R63)~=0      
                             [Probability_Matrix,R75,r75,R76,r76]=Choice(R63,r63,Probability_Matrix,Dis_Mat,x)
                         end                     
                         if length(R64)~=0  
                             [Probability_Matrix,R77,r77,R78,r78]=Choice(R64,r64,Probability_Matrix,Dis_Mat,x) 
                         end   
                     end   
                 end
                 if length(R42)~=0  
                     [Probability_Matrix,R53,r53,R54,r54]=Choice(R42,r42,Probability_Matrix,Dis_Mat,x)
                     if length(R53)~=0 
                         [Probability_Matrix,R65,r65,R66,r66]=Choice(R53,r53,Probability_Matrix,Dis_Mat,x)
                         if length(R65)~=0   
                             [Probability_Matrix,R79,r79,R710,r710]=Choice(R65,r65,Probability_Matrix,Dis_Mat,x) 
                         end
                         if length(R66)~=0 
                             [Probability_Matrix,R711,r711,R712,r712]=Choice(R66,r66,Probability_Matrix,Dis_Mat,x) 
                         end    
                     end
                     if length(R54)~=0  
                         [Probability_Matrix,R67,r67,R68,r68]=Choice(R54,r54,Probability_Matrix,Dis_Mat,x)
                         if length(R67)~=0  
                             [Probability_Matrix,R713,r713,R714,r714]=Choice(R67,r67,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R68)~=0     
                             [Probability_Matrix,R715,r715,R716,r716]=Choice(R68,r68,Probability_Matrix,Dis_Mat,x)  
                         end  
                     end   
                 end    
             end
             if length(R32)~=0      
                 [Probability_Matrix,R43,r43,R44,r44]=Choice(R32,r32,Probability_Matrix,Dis_Mat,x)
                 if length(R43)~=0   
                     [Probability_Matrix,R55,r55,R56,r56]=Choice(R43,r43,Probability_Matrix,Dis_Mat,x)
                     if length(R55)~=0   
                         [Probability_Matrix,R69,r69,R610,r610]=Choice(R55,r55,Probability_Matrix,Dis_Mat,x)
                         if length(R69)~=0  
                             [Probability_Matrix,R717,r717,r718,r718]=Choice(R69,r69,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R610)~=0  
                             [Probability_Matrix,R719,r719,R720,r720]=Choice(R610,R610,Probability_Matrix,Dis_Mat,x) 
                         end  
                     end
                     if length(R56)~=0       
                         [Probability_Matrix,R611,r611,R612,r612]=Choice(R56,r56,Probability_Matrix,Dis_Mat,x)
                         if length(R611)~=0  
                             [Probability_Matrix,R721,r721,R722,r722]=Choice(R611,r611,Probability_Matrix,Dis_Mat,x) 
                         end
                         if length(R612)~=0   
                             [Probability_Matrix,R723,r723,R724,r724]=Choice(R612,r612,Probability_Matrix,Dis_Mat,x)
                         end 
                     end  
                 end
                 if length(R44)~=0     
                     [Probability_Matrix,R57,r57,R58,r58]=Choice(R44,r44,Probability_Matrix,Dis_Mat,x)
                     if length(R57)~=0 
                         [Probability_Matrix,R613,r613,R614,r614]=Choice(R57,r57,Probability_Matrix,Dis_Mat,x)
                         if length(R613)~=0   
                             [Probability_Matrix,R725,r725,R726,r726]=Choice(R613,r613,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R614)~=0  
                             [Probability_Matrix,R727,r727,R728,r728]=Choice(R614,r614,Probability_Matrix,Dis_Mat,x)
                         end  
                     end
                     if length(R58)~=0   
                         [Probability_Matrix,R615,r615,R616,r616]=Choice(R58,r58,Probability_Matrix,Dis_Mat,x)
                         if length(R615)~=0  
                             [Probability_Matrix,R729,r729,R730,r730]=Choice(R615,r615,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R616)~=0  
                             [Probability_Matrix,R731,r731,R732,r732]=Choice(R616,r616,Probability_Matrix,Dis_Mat,x)
                         end  
                     end  
                 end  
             end  
         end  
         if length(R22)~=0      
             [Probability_Matrix,R33,r33,R34,r34]=Choice(R22,r22,Probability_Matrix,Dis_Mat,x)
             if length(R33)~=0  
                 [Probability_Matrix,R45,r45,R46,r46]=Choice(R33,r33,Probability_Matrix,Dis_Mat,x)
                 if length(R45)~=0   
                     [Probability_Matrix,R59,r59,R510,r510]=Choice(R45,r45,Probability_Matrix,Dis_Mat,x)
                     if length(R59)~=0   
                         [Probability_Matrix,R617,r617,R618,r618]=Choice(R59,r59,Probability_Matrix,Dis_Mat,x)
                         if length(R617)~=0   
                             [Probability_Matrix,R733,r733,R734,r734]=Choice(R617,r617,Probability_Matrix,Dis_Mat,x) 
                         end
                         if length(R618)~=0  
                             [Probability_Matrix,R735,r735,R736,r736]=Choice(R618,r618,Probability_Matrix,Dis_Mat,x)
                         end    
                     end
                     if length(R510)~=0 
                         [Probability_Matrix,R619,r619,R620,r620]=Choice(R510,r510,Probability_Matrix,Dis_Mat,x)
                         if length(R619)~=0 
                             [Probability_Matrix,R737,r737,R738,r738]=Choice(R619,r619,Probability_Matrix,Dis_Mat,x) 
                         end  
                         if length(R620)~=0 
                             [Probability_Matrix,R739,r739,R740,r740]=Choice(R620,r620,Probability_Matrix,Dis_Mat,x)  
                         end   
                     end  
                 end
                 if length(R46)~=0  
                     [Probability_Matrix,R511,r511,R512,r512]=Choice(R46,r46,Probability_Matrix,Dis_Mat,x)
                     if length(R511)~=0  
                         [Probability_Matrix,R621,r621,R622,r622]=Choice(R511,r511,Probability_Matrix,Dis_Mat,x)
                         if length(R621)~=0  
                             [Probability_Matrix,R741,r741,R742,r742]=Choice(R621,r621,Probability_Matrix,Dis_Mat,x) 
                         end
                         if length(R622)~=0    
                             [Probability_Matrix,R743,r743,R744,r744]=Choice(R622,r622,Probability_Matrix,Dis_Mat,x)
                         end    
                     end
                     if length(R512)~=0   
                         [Probability_Matrix,R623,r623,R624,r624]=Choice(R512,r512,Probability_Matrix,Dis_Mat,x)
                         if length(R623)~=0 
                             [Probability_Matrix,R745,r745,R746,r746]=Choice(R623,r623,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R624)~=0  
                             [Probability_Matrix,R747,r747,R748,r748]=Choice(R624,r624,Probability_Matrix,Dis_Mat,x)
                         end   
                     end   
                 end   
             end
             if length(R34)~=0  
                 [Probability_Matrix,R47,r47,R48,r48]=Choice(R34,r34,Probability_Matrix,Dis_Mat,x)
                 if length(R47)~=0  
                     [Probability_Matrix,R513,r513,R514,r514]=Choice(R47,r47,Probability_Matrix,Dis_Mat,x)
                     if length(R513)~=0 
                         [Probability_Matrix,R625,r625,R626,r626]=Choice(R513,r513,Probability_Matrix,Dis_Mat,x)
                         if length(R625)~=0   
                             [Probability_Matrix,R749,r749,R750,r750]=Choice(R625,r625,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R626)~=0  
                             [Probability_Matrix,R751,r751,R752,r752]=Choice(R626,r626,Probability_Matrix,Dis_Mat,x) 
                         end  
                     end
                     if length(R514)~=0   
                         [Probability_Matrix,R627,r627,R628,r628]=Choice(R514,r514,Probability_Matrix,Dis_Mat,x)
                         if length(R627)~=0
                             [Probability_Matrix,R753,r753,R754,r754]=Choice(R627,r627,Probability_Matrix,Dis_Mat,x)
                         end
                         if length(R628)~=0 
                             [Probability_Matrix,R755,r755,R756,r756]=Choice(R628,r628,Probability_Matrix,Dis_Mat,x) 
                         end 
                     end  
                 end
                 if length(R48)~=0 
                     [Probability_Matrix,R515,r515,R516,r516]=Choice(R48,r48,Probability_Matrix,Dis_Mat,x)