tc.m

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

%------------------------------------------------------------------------
%                       SWARM SIMULATION PROGRAM 
%                        DESIGNED BY Jun Lu
%                   Intelligent and Complex Systems Lab
%               HuaZhong University of Science and Technology
%------------------------------------------------------------------------

%This program works on the line of sight testing
%判断LOS
function G=TC(obs_cent_x,obs_cent_y,obs_rad,x)%输入障碍物的圆点（obs_cent_x,obs_cent_y），半径obs_rad，x为机器人的初始化坐标
xc=obs_cent_x;
yc=obs_cent_y;
Rob_num=length(x);%机器人的个数
G=zeros(Rob_num);%zeros:产生全为0的矩阵 ，产生机器人个数的全0矩阵
dl=sqrt((x(1,1)-xc)^2 + (x(1,2)-yc)^2);

for i=1:Rob_num
    for j=1:Rob_num
        xs=x(i,1);%第1行，第1列    x为机器人的初始化坐标，  n*2:n行：表示机器人的个数； 共2列，第一列为x轴坐标，第二列为y轴坐标
        ys=x(i,2);      %coordinates of starting points 起点坐标
        xe=x(j,1);
        ye=x(j,2);      %coordinates of ending points 终点坐标
        A=ys-ye;
        B=xe-xs;
        C=ye*xs-xe*ys;  %A,B,C are coefficients of line passing through (xs,ys) and (xe,ye)

    
            if ((i~=j) & (xe>=xs)) % here only test those points which have bigger x value;只考虑每个机器人前面的机器人
                if xs==xe   %并排
                    Dist_C=abs(xc-xs);%  障碍物圆点到起点与终点连线的距离
                    cross_x=xs;    %障碍物到连线的垂线的垂点
                    cross_y=yc;
                elseif ys==ye
                    Dist_C=abs(yc-ys);
                    cross_x=xc;
                    cross_y=ys;
                else
                    Dist_C=abs(A*xc+B*yc+ye*xs-xe*ys)/sqrt(A^2+B^2);
                    cross_x=-(A*B*yc-B*B*xc+A*C)/(A^2+B^2);
                    cross_y=-(A*B*xc-A*A*yc+B*C)/(A^2+B^2);
                end  
                if Dist_C>=obs_rad  % the distance check makes sure no cross point with the circle;如果障碍物圆点到起点与终点连线的距离大于障碍物半径，则相连
                    G(i,j)=1;
                    G(j,i)=1;
                else %调用check_on_line函数，判断垂点是在起点和终点的连线上，还是在连线的延长线上
                    sign_on_line=check_on_line(xs,ys,xe,ye,cross_x,cross_y) ;% check if the point is inside (xs,ys) and (xe,ye)
                    if (sign_on_line==0)
                        G(i,j)=1;
                        G(j,i)=1;
                    end
                end
            end
        end
    end

if(dl<obs_rad)%如果leader在障碍物内
    for i=1:Rob_num
        G(1,i)=0;
        G(i,1)=0;
    end
end