choice.m

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

%------------------------------------------------------------------------
%                       SWARM SIMULATION PROGRAM 
%                        DESIGNED BY Jun Lu
%                   Intelligent and Complex Systems Lab
%               HuaZhong University of Science and Technology
%------------------------------------------------------------------------
function [Probability_Matrix,Left,left,Right,right]=Choice(First,fl,Probability_Matrix,Dis_Mat,x)
%First包含id和相应x轴距离的值，2×n的矩阵,fl为first leader
Left=[];
Right=[];
left=0;
right=0;
L1=[];
R1=[];

if length(First(1,:))>1
    %划分为Left,Right两个区域
    lid=1;
    rid=1;
    for i=1:length(First)
        %if First(2,i)~=0
            if x(First(1,i),2)>x(fl,2)|(x(First(1,i),2)==x(fl,2)&x(First(1,i),1)~=x(fl,1))%判断fl的左子树          
                Left(1,lid)=First(1,i);
                Left(2,lid)=Dis_Mat(fl,First(1,i));
                lid=lid+1;
            elseif x(First(1,i),2)<x(fl,2) %判断fl的右子树
                Right(1,rid)=First(1,i);
                Right(2,rid)=Dis_Mat(fl,First(1,i));
                rid=rid+1;
            end
            % end
    end
num_LL=0;%和leader相连的左区间agent数
num_LR=0;%和leader相连的左区间agent数
%-------------------------在新划分的区域中确定fl是否有与其相连的agent区域
if length(Left)~=0 
   for i=1:length(Left(1,:)) 
        if Left(2,i)~=0
        num_LL=num_LL+1;
        break
        end
    end
end
if length(Right)~=0  
    for i=1:length(Right(1,:)) 
        if Right(2,i)~=0
        num_LR=num_LR+1;
        break
        end
    end
end
%----------leader没有左右区域
if(num_LL==0&num_LR==0)
     Temp=[];
     Reset=[];
    k=1;
    rfl=0;
    rFirst=[];
    for i=1:length(First(1,:))
        if First(1,i)~=fl;
            Temp(1,k)=First(1,i);
            Temp(2,k)=x(First(1,i),1);
            k=k+1;
        end
    end
    temp=max(Temp(2,:));  %确定rfl
    for j=1:length(Temp(1,:))
        if temp==Temp(2,j)
            rfl=Temp(1,j);
            break
        end
    end
    for j=1:length(Temp(1,:))        
            Reset(1,j)=Temp(1,j);
            Reset(2,j)=0;
        end
    for j=1:length(Reset(1,:))
        rFirst(1,j)=Reset(1,j);
        rFirst(2,j)=Dis_Mat(rfl,Reset(1,j));
    end
 Left=[];
 Right=[];
 left=0;
 right=0;
[Probability_Matrix,Left,left,Right,right]=Choice(rFirst,rfl,Probability_Matrix,Dis_Mat,x);
%%--------------------------------------------------------leader和左区域agent相连的情况,确定left
else 
 if(num_LL~=0)
  l1id=1;
    for i=1:length(Left(1,:)) 
        if Left(2,i)~=0
            L1(1,l1id)=Left(1,i);
            L1(2,l1id)=Dis_Mat(fl,Left(1,i));
            l1id=l1id+1;
        end
     end
%确定leader的左节点

      ltemp=min(L1(2,:));
       for j=1:length(L1(1,:))%最小值对应的agent的id
            if ltemp==L1(2,j)
            left=L1(1,j);
            Probability_Matrix(left,fl)=1;%找到了与leader相连的左节点
            break
            end
       end
  %%-----------------------------------------------------leader不和左区域agent相连的情况,确定l
 else 
 if  length(Left)~=0 
    Temp=[];
    k=1;
    for i=1:length(Left(1,:))
        Temp(1,k)=Left(1,i);
        Temp(2,k)=x(Left(1,i),1);
        k=k+1;
    end
    ltemp=max(Temp(2,:));
    for j=1:length(Temp(1,:))
        if ltemp==Temp(2,j)
            left=Temp(1,j);
            Probability_Matrix(left,fl)=0;
            break
        end
    end
end
end
%%--------------------------------------------------------leader和左区域agent相连的情况,确定left
if(num_LR~=0)
  r1id=1;
    for i=1:length(Right(1,:)) 
        if Right(2,i)~=0
            R1(1,r1id)=Right(1,i);
            R1(2,r1id)=Dis_Mat(fl,Right(1,i));
            r1id=r1id+1;
        end
     end
     
%确定leader的左节点

      rtemp=min(R1(2,:));
       for j=1:length(R1(1,:))%最小值对应的agent的id
            if rtemp==R1(2,j)
            right=R1(1,j);
            Probability_Matrix(right,fl)=1;%找到了与leader相连的左节点
            break
            end
       end
%%-----------------------------------------------------leader不和左区域agent相连的情况,确定l
 else
 if  length(Right)~=0 
    Temp=[];
    k=1;
    for i=1:length(Right(1,:))
        Temp(1,k)=Right(1,i);
        Temp(2,k)=x(Right(1,i),1);
        k=k+1;
    end
    rtemp=max(Temp(2,:));
    for j=1:length(Temp(1,:))
        if rtemp==Temp(2,j)
            right=Temp(1,j);
            break
        end
    end
end
end
end
end