【目标识别】自适应多机器人编队，可在运动和能见度约束下包围和跟踪目标（Matlab代码实现）

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

近年来，以机器人为代表的人工智能产业逐渐走进了各行各业，单机器人系统在很多方面不能满足人们生产和生活的需要，多机器人协作控制成为了当今机器人研究的热门方向。多机器人编队控制是多机器人协作研究的基础，传统多机器人编队的控制方式相对复杂，实现起来较为困难，且不能很好的适应外界环境。在实际应用中，多机器人队形需要面临复杂的环境，且经常需要进行队形变换，实际的需求给我们研究多机器人编队控制算法的自适应方面提出了更高的要求。因此，本文为了解决包围和跟踪目标的问题需要具有足够运动策略的多个代理。我们考虑一个带有标准摄像头的独轮车机器人团队。机器人必须保持所需的封闭结构，同时处理其非完整运动约束。参考编队轨迹还必须通过克服相机的有限视野来保证目标的永久可见性。 我们提出了一种新颖的方法来表征机器人轨迹上的条件，同时考虑到运动和视觉约束。 我们还提出在线和离线运动规划策略，以解决封闭和跟踪目标任务所涉及的限制。 这些策略基于保持具有可变尺寸的地层形状，或者基于以灵活的形状保持地层的大小。

📚2 运行结果

 

 

 部分代码：

%mainMultirobotConstrainedEnclosing.m
%G. Lopez-Nicolas. University Zaragoza. 2019

%This implementation addresses the problem of enclosing and tracking a
%target with multiple robots. We consider a team of unicycle robots with a
%standard camera on board. The robots must maintain the desired enclosing
%formation while dealing with their nonholonomic motion constraints. The
%reference formation trajectories must also guarantee permanent visibility
%of the target by overcoming the limited field of view of the cameras. 
%Several motion strategies are implemented to address this task. These are
%based on maintaining the formation shape with variable size
%(distance-based) or, alternatively, on maintaining the size of the
%formation with flexible shape (theta-based).
%
%Related paper:
%Title: Adaptive multirobot formation to enclose and track a target with
%motion and visibility constraints
%Authors: Gonzalo Lopez-Nicolas, Miguel Aranda and Youcef Mezouar

%Main program: mainMultirobotConstrainedEnclosing.m
%Required files:
% betweenPi.m
% computeBi.m
% computeDi.m
% computeThetai.m
% drawRobots.m
% generateTrajectory.m
% optimizeACd.m
% optimizeACtheta.m
% optimizeData.m

%Notation: In general 'v_xxx' refers to one-dimensional vector of a
%variable xxx evolving with time. A matrix of several variables evolving
%with time would be 'm_xxx'

clear all
close all

%Selection of target trajectory
selection= 'e'; %Ellipse
% selection= 's'; %Sinusoid
% selection= '8'; %Eight
% selection= 'p'; %Spiral
% selection= 'r'; %Rectilinear
% selection= 'c'; %Circle
% selection= 'c2';%Circle small

%Selection of strategy to comply with motion and FOV constraints:
% d-based: Modify formation scale/size (distance of robots to target)
% theta-based: Modify formation shape (location angle of robots with
% respect to target) 
strategy= 'd';
% strategy= 'theta';

%Selection of method: Each strategy implements different alternatives
% method = 0 %Constant prescribed formation. No strategy is applied so
% constraints compliance is not guaranteed.
% method = 1 %Method that defines a constant formation in order to obey
% motion and FOV constraints. Strategy 'd' selects a constant value of
% scale d (v_dr), normally different than the prescribed one (d0) with the
% prescribed angles thetar0. Strategy 'theta' selects constant values of theta
% (m_thetar), normally different than the prescribed one (m_thetar0) with the
% prescribed scale dr0. 
% method = 2 %Method that defines a variable formation in order to obey
% motion and FOV constraints. Strategy 'd' defines the evolution of d
% (v_dr) with the prescribed thetar0. Strategy 'theta' defines the
% evolution of theta (m_thetar) with the prescribed scale dr0. 
%
% method = 0; %No strategy
% method = 1; %Design constant values (d or theta) to obey constraints
method = 2; %Design variable values (d or theta) to obey constraints

%Several figures show the results of the simulation, if animate=1 an
%animation of the formation of robots in navigation is also shown
animate = 0;
% animate = 1;

%Definition of the prescribed formation
%Number N of observing cameras (enclosing robots)
N = 5;

%Prescribed formation scale (distance of robots to target)
dr0 = 5;

%Angular position theta of each robot with respect to the target reference
%from -pi to pi. By default the angles of the robots are evenly distributed
thetar0 = -pi:2*pi/N:pi-2*pi/N;

%Angle of the camera field of view (FOV), with limits (-betaMax,betaMax) 
betaMax = 30*pi/180;

%-------------------------------------------
%Configuration of some of the examples tested:
%(One example per line, uncomment any line and execute the program to see the result)

% selection= 'e'; strategy= 'd'; method = 0; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'd'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'd'; method = 2; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'theta'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'theta'; method = 2; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

% selection= 'e'; strategy= 'd'; method = 1; dr0 = 5; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'd'; method = 2; dr0 = 5; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'theta'; method = 1; dr0 = 5; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'e'; strategy= 'theta'; method = 2; dr0 = 5; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

% selection= 's'; strategy= 'd'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 's'; strategy= 'd'; method = 2; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 's'; strategy= 'theta'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 's'; strategy= 'theta'; method = 1; dr0 = 10; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 's'; strategy= 'theta'; method = 2; dr0 = 10; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

% selection= '8'; strategy= 'd'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= '8'; strategy= 'd'; method = 2; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= '8'; strategy= 'theta'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= '8'; strategy= 'theta'; method = 1; dr0 = 10; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= '8'; strategy= 'theta'; method = 2; dr0 = 10; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

% selection= 'p'; strategy= 'd'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'p'; strategy= 'd'; method = 2; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'p'; strategy= 'theta'; method = 1; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'p'; strategy= 'theta'; method = 2; dr0 = 5; N = 5; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

% selection= 'p'; strategy= 'd'; method = 1; dr0 = 5; N = 4; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'p'; strategy= 'd'; method = 2; dr0 = 5; N = 4; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'p'; strategy= 'theta'; method = 1; dr0 = 5; N = 4; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'p'; strategy= 'theta'; method = 2; dr0 = 5; N = 4; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

% selection= 'r'; strategy= 'd'; method = 1; dr0 = 5; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'c'; strategy= 'd'; method = 1; dr0 = 5; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'c'; strategy= 'theta'; method = 1; dr0 = 10; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'c2'; strategy= 'd'; method = 1; dr0 = 10; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;
% selection= 'c2'; strategy= 'theta'; method = 1; dr0 = 10; N = 8; thetar0=-pi:2*pi/N:pi-2*pi/N; betaMax= 30*pi/180;

%-------------------------------------------
disp('------------------------------------');
disp(['Selected trajectory: ', selection]);
disp(['Selected strategy: ', strategy, ', with method ', num2str(method) ]);
disp(['Number of robots N = ', num2str(N)]);
disp(['Prescribed formation scale d = ', num2str(dr0,2)]);
disp(['Angular half-FOV limits beta [deg] = ', num2str(betaMax*180/pi,2)]);
disp('Prescribed angular positions theta of robots [deg]:');
disp(['(', num2str(thetar0*180/pi),')']);
%-------------------------------------------
%Generation of the target trajectory
delta=0.1; %Step time in each iteration
[t,v_vt,v_wt, Tmax]=generateTrajectory(delta, selection);
nt=length(t);

%Target motion computation
v_fit=cumsum(v_wt.*delta);
v_dxt=v_vt.*delta.*cos(v_fit);
v_dyt=v_vt.*delta.*sin(v_fit);

v_xt=cumsum(v_dxt);
v_yt=cumsum(v_dyt);
%-------------------------------------------
%Data preparation
if strcmp(strategy,'theta')
    m_thetar0=repmat(thetar0,nt,1);
    m_v_vt=repmat(v_vt,1,N);
    m_v_wt=repmat(v_wt,1,N);

    v_dr0=dr0*ones(nt,1);
    m_dr0=repmat(v_dr0,1,N);

    xr0=dr0*cos(thetar0);
    yr0=dr0*sin(thetar0);
    m_xr0=v_dr0*cos(thetar0);
    m_yr0=v_dr0*sin(thetar0);
end

if strcmp(strategy,'d')
    v_betaMin=-betaMax*ones(nt,1);
    [m_dwc,v_dwcMint]=computeDi(delta,t,v_betaMin,betaMax,v_vt,v_wt);
    m_dwc=abs(m_dwc);
    dmin=min(min(v_dwcMint));
    dwmin=fix(dmin*100)/100;%Round off
    kLMax=1/dmin;
    v_kLMax=kLMax*ones(nt,1);
end

%-------------------------------------------
%Application of the selected strategy and method

if method==2
    %Load configuration parameters for optimization
    [vA,vC,optimize] = optimizeData(selection,strategy,method,dr0,N,thetar0,betaMax,delta);

    if optimize == -2
        %Change to method 1 because method 2 is not neccesary
        method = 1;
        disp('Changing to method = 1.');
    end
end

if strcmp(strategy,'d')

    %Formation scale dr0 (distance of robots to target)
    if method>0,
        %maximum constant d that obeys constraints
        if dmin==Inf,
            dr0 = 10;
        else
            dr0= dmin ;
        end
    %else: d constant, without strategy.
    end

    v_dwcMin=dmin*ones(nt,1);

    v_dr0=dr0*ones(nt,1);
    xr0=dr0*cos(thetar0);
    yr0=dr0*sin(thetar0);
    m_xr0=v_dr0*cos(thetar0);
    m_yr0=v_dr0*sin(thetar0);

    v_dr=v_dr0;
    v_kr0=1./v_dr;

    
    if method==2 %maximize variable d that obeys constraints
        m_kwc=1./m_dwc;
        areakMax=sum(1/dmin-m_kwc);
        v_kLMax=1./v_dwcMin;
        
        %Using optimization parameters (vA, vC) for d-strategy
        [A,C,krCost,kr] = optimizeACd(vA,vC,v_kLMax,dmin,N,nt,delta,t,thetar0,v_vt,v_wt,betaMax);
        %Percentage of covered area with respect to the constraint limit
        areak100=100*krCost/areakMax;

        v_dr=1./kr;
    end %if method==2

    v_kr=1./v_dr;
    m_xr=v_dr*cos(thetar0);
    m_yr=v_dr*sin(thetar0);

    %Check FOV constraint with obtained result
    [m_betari, m_complyBmaxr] = computeBi(delta,t,v_dr,thetar0,thetar0,v_vt,v_wt,betaMax,0);
end %if strategy == 'd',

if strcmp(strategy,'theta')

    if method==0, %Prescribed constant theta, without strategy.
        thetar=thetar0;
        m_xr=m_xr0;
        m_yr=m_yr0;
        m_thetar=repmat(thetar,nt,1);
    end

    %Check FOV constraint with initial configuration
    [m_betari0, m_complyBmaxr0] = computeBi(delta,t,v_dr0,thetar0,m_thetar0,v_vt,v_wt,betaMax,0);
    m_complyBmax0=repmat( sum(m_complyBmaxr0)==nt , nt,1);

    %Limits for theta to obey FOV constraints
    [m_thetarMin, m_thetarMax] = computeThetai(betaMax,dr0,m_thetar0,m_v_vt,m_v_wt,0);

    if sum(sum([m_thetarMin, m_thetarMax]))==0 && method>0
        %It does not exist theta that holds FOV constraints
        disp('Try reducing the formation scale or increasing angle beta of FOV');
        return
    end

    if method==1, 
        %Look for constant values of theta to obey constraints 
        %It may exist solution or not
        m_thetarMaxT = repmat( min(m_thetarMax), nt, 1);
        m_thetarMinT = repmat( max(m_thetarMin), nt, 1);
  
        conditionCloser = abs(betweenPi(m_thetarMinT - m_thetar0)) < abs(betweenPi(m_thetarMaxT - m_thetar0));
        m_thetarProx = m_thetarMinT .* conditionCloser + m_thetarMaxT .* (~conditionCloser);

        %Do not modify those thetar0 that already obey FOV constraint
        m_thetar = m_thetar0 .* m_complyBmax0 + m_thetarProx .* (~m_complyBmax0);

        m_xr=dr0*cos(m_thetar);
        m_yr=dr0*sin(m_thetar);

    end %method==1

    if method==2 
        %Find variable theta (the closest to thetar0) that obeys constraints
        m_thetarMaxT = repmat( min(m_thetarMax), nt, 1);
        m_thetarMinT = repmat( max(m_thetarMin), nt, 1);
        
        [A,C,v_thetarCost,m_thetar]=optimizeACtheta(vA,vC,m_complyBmax0,dr0,N,nt,delta,t,m_thetar0,v_vt,v_wt,betaMax);

        m_xr=dr0*cos(m_thetar);
        m_yr=dr0*sin(m_thetar);
    end %method==2

    %Check FOV constraint with obtained result
    [m_betari, m_complyBmaxr] = computeBi(delta,t,v_dr0,thetar0,m_thetar,v_vt,v_wt,betaMax,0);
    
end %if strategy == 'd',

complyBmax= sum(m_complyBmaxr)==nt;
if sum(complyBmax)<N
    disp('The target left the FOV of some robot.');
    switch method
        case 0
            disp('No strategy was used (method=0).');
        case 1
            disp('No solution exists for constant formation shape (method=1).');
        case 2
            disp('No solution was found with variable formation shape (method=2).');
            disp('Try increasing optimization range parameters (vA,vC) in optimizeData.m');
            if strcmp(strategy,'theta')
                icomplyBmax = find(complyBmax<1);
                m_thetar(:,icomplyBmax)=m_thetar0(:,icomplyBmax);
                [m_betari, m_complyBmaxr] = computeBi(delta,t,v_dr0,thetar0,m_thetar,v_vt,v_wt,betaMax,0);
                m_xr=dr0*cos(m_thetar);
                m_yr=dr0*sin(m_thetar);
            end

    end
end

%----------------------------------------------------------------------
%Compute reference trajectories with the resultant parameters of the
%selected strategy
m_v_xt=repmat(v_xt,1,N);
m_v_yt=repmat(v_yt,1,N);
m_v_fit=repmat(v_fit,1,N);

if strcmp(strategy,'d')
    m_v_dr=repmat(v_dr,1,N);
    m_thetar0=repmat(thetar0,nt,1);
    m_v_vt=repmat(v_vt,1,N);
    m_v_wt=repmat(v_wt,1,N);
else %if strategy == 'theta',
    m_v_dr=repmat(v_dr0,1,N);
end

v_xr= cos(m_v_fit).*m_xr - sin(m_v_fit).*m_yr + m_v_xt;
v_yr= sin(m_v_fit).*m_xr + cos(m_v_fit).*m_yr + m_v_yt;

m_v_dvt=diff(m_v_vt./delta);
m_v_dvt=[m_v_dvt; m_v_dvt(end,:)];
m_v_dwt=diff(m_v_wt./delta);
m_v_dwt=[m_v_dwt; m_v_dwt(end,:)];

if strcmp(strategy,'d')
    m_dthetar0=zeros(nt, N); %Constant theta
    
    fsc= m_v_vt.*sin(m_v_fit) + m_v_dr.*cos(m_v_fit+m_thetar0).*m_v_wt;
    fcs= m_v_vt.*cos(m_v_fit) - m_v_dr.*sin(m_v_fit+m_thetar0).*m_v_wt;
else %if strategy == 'theta',
    m_thetarS=zeros(nt,N);
    m_dthetar= diff(m_thetar/delta);
    m_dthetar=[m_dthetar; m_dthetar(end,:)];
    
    fsc= m_v_vt.*sin(m_v_fit) + m_v_dr.*cos(m_v_fit+m_thetar).*m_v_wt;
    fcs= m_v_vt.*cos(m_v_fit) - m_v_dr.*sin(m_v_fit+m_thetar).*m_v_wt;
end

m_ddr= diff(m_v_dr/delta);
m_ddr=[m_ddr; m_ddr(end,:)];

m_dddr= diff(m_ddr/delta);
m_dddr=[m_dddr; m_dddr(end,:)];

if strcmp(strategy,'d'),
    v_dxr=fcs+m_ddr.*cos(m_v_fit+m_thetar0);
    v_dyr=fsc+ m_ddr.*sin(m_v_fit+m_thetar0);
else %if strategy == 'theta',
    v_dxr=fcs+m_ddr.*cos(m_v_fit+m_thetar) - m_v_dr.*sin(m_v_fit+m_thetar).*m_dthetar;
    v_dyr=fsc+ m_ddr.*sin(m_v_fit+m_thetar) + m_v_dr.*cos(m_v_fit+m_thetar).*m_dthetar;
end

v_fir=atan2( v_dyr , v_dxr);
v_fir=unwrap(v_fir);

v_vr= sqrt(v_dxr.*v_dxr+v_dyr.*v_dyr);

v_wr=diff(v_fir/delta); 
v_wr=[v_wr; v_wr(end,:)];

%----------------------------------------------------------------------
%Main loop of the formation evolution

%Initial formation (1 unit behind the reference)
x=v_xr(1,:) - 1;
y=v_yr(2,:);
fi=v_fir(1,:);

%Arrays to save data
v_x=zeros(nt,N);
v_y=zeros(nt,N);
v_fi=zeros(nt,N);

v_xe=zeros(nt,N);
v_ye=zeros(nt,N);
v_fie=zeros(nt,N);
v_roe=zeros(nt,N);
v_alfe=zeros(nt,N);
v_chie=zeros(nt,N);

v_v=zeros(nt,N);
v_w=zeros(nt,N);

for it=1:nt,

    %Current robot locations
    v_x(it,:)=x;
    v_y(it,:)=y;
    v_fi(it,:)=fi;

    %Tracking error relative to reference trajectory
    %In Cartesian coordinates
    xetr= x-v_xr(it,:);
    yetr= y-v_yr(it,:);
    xe= cos(v_fir(it,:)) .* xetr + sin(v_fir(it,:)) .* yetr ;
    ye=-sin(v_fir(it,:)) .* xetr + cos(v_fir(it,:)) .* yetr ;
    %In polar coordinates
    chie=atan2(ye,xe) ;
    roe= sqrt(xe.^2 + ye.^2);
    chir=atan2(v_yr(it,:),v_xr(it,:));
    fie=fi-v_fir(it,:);
    alfe=fie-chie -pi;

    %Tracking control to be implemented
    %v= ... ;
    %w= ... ;

    %Instead, we consider perfect tracking and the reference velocities
    %(v_vr, v_wr) are applied to the robots:
    v=v_vr(it,:);
    w=v_wr(it,:);

    %Motion of the robots
    fi=fi + w*delta;
    Dy= zeros(1,N);
    Dx= v*delta;
    nz=find(w ~= 0);
    if ~isempty(nz),
        L= v(nz)./w(nz); %Curvature radius (radians)
        Dy(nz)= L -L.*cos(w(nz)*delta);
        Dx(nz)= L.*sin(w(nz)*delta);
    end
    x= x +Dx .*cos(fi) -Dy .*sin(fi);
    y= y +Dx .*sin(fi) +Dy .*cos(fi);

    %Store data
    v_v(it,:)=v;
    v_w(it,:)=w;
    v_xe(it,:)=xe;
    v_ye(it,:)=ye;
    v_fie(it,:)=fie;
    v_roe(it,:)=roe;
    v_alfe(it,:)=alfe;
    v_chie(it,:)=chie;

end %Main loop
%----------------------------------------------------------------------

%----------------------------------------------------------------------
%%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% 
% FIGURES % % FIGURES % % FIGURES % % FIGURES % % FIGURES % % FIGURES % 
%%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% %%%%%%%%%%% 
%----------------------------------------------------------------------

%----------------------------------------------------------------------
%----------------------------------------------------------------------
% scrsz = get(0,'ScreenSize');
% figure('position',[51 51 scrsz(3)-100 scrsz(4)-150]);
f=figure('visible', 'off');
%----------------------------------------------------------------------
subplot(2,3,1);
plot(t,v_xt,'r','LineWidth',3);
hold;
plot(t,v_xr,'k','LineWidth',1);
plot(t,v_x,'b','LineWidth',1);
xlabel('time');
legend('target','robots');

title('x-coordinate');
%----------------------------------------------------------------------
subplot(2,3,2);
plot(t,v_yt,'r','LineWidth',3);
hold;
plot(t,v_yr,'k','LineWidth',1);
plot(t,v_y,'b','LineWidth',1);
xlabel('time');
title('y-coordinate');
%----------------------------------------------------------------------
subplot(2,3,3);
plot(t,v_fit*180/pi,'r','LineWidth',3);
hold;
plot(t,v_fir*180/pi,'k','LineWidth',1);
plot(t,v_fi*180/pi,'b','LineWidth',1);
xlabel('time');
title('phi-coordinate');
%----------------------------------------------------------------------
subplot(2,3,4);
plot(t,v_vt,'r','LineWidth',3);
hold;
plot(t,v_vr,'k','LineWidth',1);
plot(t,v_v,'b','LineWidth',1);
xlabel('time');
title('Linear velocity: v');
%----------------------------------------------------------------------
subplot(2,3,5);
plot(t,v_wt*180/pi,'r','LineWidth',3);
hold;
plot(t,v_wr*180/pi,'k','LineWidth',1);
plot(t,v_w*180/pi,'b','LineWidth',1);
xlabel('time');
title('Angular velocity: w');
%----------------------------------------------------------------------
subplot(2,3,6);
plot(v_xt,v_yt,'r','LineWidth',3);
hold;
plot(v_xr,v_yr,'k','LineWidth',1);
plot(v_x,v_y,'b','LineWidth',1);

axis equal
title('Top view: x-y');

print('-djpeg','./VariablesEvolution.jpg');
close(f);
%----------------------------------------------------------------------
%----------------------------------------------------------------------
colors=get(gca,'ColorOrder');
if N>size(colors,1),
    colors=repmat(colors, ceil(N/size(colors,1) ) ,1);
end
%----------------------------------------------------------------------
%----------------------------------------------------------------------

f=figure('visible', 'off');
%betarisGT= -v_fir-m_thetar0+pi+atan2(m_v_yt-v_yr,m_v_xt-v_xr);%Ground truth
plot([1 Tmax],[betaMax, betaMax]*180/pi,'k:','LineWidth',3);
hold;
plot([1 Tmax],-[betaMax, betaMax]*180/pi,'k:','LineWidth',3);
for ii=1:N,
    colori=colors(ii,:);
    plot(t,m_betari(:,ii)*180/pi,'color',colori,'LineWidth',3);
    %plot(t(1:100:end),entrepi(unwrap(betarisGT(1:100:end,ii)))*180/pi,'*','color',colori);
end

xlabel('time');
ylabel('FOV [degrees]');
title('Evolution of the target projection in the cameras FOV between (-betaMax, betaMax)');
print('-djpeg','./FOV.jpg');
close(f);
%----------------------------------------------------------------------
%----------------------------------------------------------------------
if strcmp(strategy,'d')
    f=figure('visible', 'off');
    plot(t,v_kLMax,'r','LineWidth',3);
    hold;
    plot(t,1./m_dwc,'b','LineWidth',3);
    plot(t,v_kr,'k','LineWidth',3);

    legend('kL','kwc','kr');
    xlabel('time');
    ylabel('Inverse formation radius');
    title('Evolution of the inverse formation radius (kappa=1/d)');
else %strategy == 'theta',
    f=figure('visible', 'off');
    plot(t,1./v_dr0,'k','LineWidth',3);

    legend('kr0');
    xlabel('time');
    ylabel('Inverse formation radius');
    title('Evolution of the PRESCRIBED inverse formation radius 1/dr0');
end
print('-djpeg','./InverseRadius.jpg');
close(f);
%----------------------------------------------------------------------
%----------------------------------------------------------------------
if strcmp(strategy,'theta')
    f=figure('visible', 'off');
    hold;

for ii=1:N,
    colori=colors(ii,:);
    plot(t, m_thetarMax(:,ii)*180/pi,'--','color',colori,'LineWidth',3);
    plot(t, m_thetarMin(:,ii)*180/pi,':','color',colori,'LineWidth',3);
    plot(t, betweenPi(m_thetar(:,ii))*180/pi,'color',colori,'LineWidth',1);
end
    legend('thetaMax', 'thetaMin','thetar');

    xlabel('time');
    ylabel('theta [degrees]');
    title('Evolution of angle theta of each robot in the formation');
else %strategy == 'd',
    f=figure('visible', 'off');
    hold;
    for ii=1:N,
        colori=colors(ii,:);
        plot(t, betweenPi(m_thetar0(:,ii))*180/pi,'color',colori,'LineWidth',3);
    end
    
    legend('thetar0');
    xlabel('time');
    ylabel('theta [degrees]');
    title('Evolution of the PRESCRIBED angle theta of each robot in the formation');
end %if strategy == 'theta',
print('-djpeg','./AngleTheta.jpg');
close(f);
%----------------------------------------------------------------------
%----------------------------------------------------------------------
f=figure('visible', 'off');
plot(v_xt,v_yt,'r','LineWidth',3);
hold;
plot(v_xr,v_yr,'k','LineWidth',1);

np=5; %Plot only every np steps
itr=1:round(Tmax/np/delta):round(Tmax/delta);

%Polygon of the reference formation
plot([v_xr(itr,:),v_xr(itr,1)]',[v_yr(itr,:),v_yr(itr,1)]','k-');
plot([v_xr(itr,1),v_xt(itr)]',[v_yr(itr,1),v_yt(itr)]','k-');

if strcmp(strategy,'theta')
    pipi=(-pi:0.01:pi)';
    for iitr=itr,
        %Draw a circle along the formation
        xcircf= dr0*cos(pipi) + v_xt(iitr);
        ycircf= dr0*sin(pipi) + v_yt(iitr);
        plot(xcircf', ycircf',':k','LineWidth',1);
    end
end

%Draw FoV
for ii=1:N, %FoV of all the robots
    drawRobots([v_xr(itr,ii),v_yr(itr,ii),v_fir(itr,ii)], 'k-', 1);
    drawRobots([v_xt(itr),v_yt(itr),v_fit(itr)], 'r-', 1);
    plot(v_xt(itr),v_yt(itr), '*r');

    colori=colors(ii,:);
    for iitr=itr,
        betari= v_fir(iitr,ii)+thetar0(1,ii)-pi+betaMax : -betaMax/10 : v_fir(iitr,ii)+thetar0(1,ii)-pi-betaMax;
        xcirc= m_v_dr(iitr,ii)*cos(betari) + v_xr(iitr,ii);
        ycirc= m_v_dr(iitr,ii)*sin(betari) + v_yr(iitr,ii);
        plot([v_xr(iitr,ii),xcirc,v_xr(iitr,ii)]', [v_yr(iitr,ii),ycirc,v_yr(iitr,ii)]','-','color',colori,'LineWidth',2);
    end
end
axis equal
title('Evolution of the reference formation. Top view (x-y)');
print('-djpeg','./TopView.jpg');
close(f);
%----------------------------------------------------------------------
%----------------------------------------------------------------------
if animate == 1

    figure('visible', 'off');
    plot(v_xt,v_yt,'r','LineWidth',3);
    hold;
    plot(v_xr,v_yr,'k','LineWidth',1);

    np=5; %Plot only every np steps
    itr=1:round(Tmax/np/delta):round(Tmax/delta);

    %Polygon of the reference formation
    plot([v_xr(itr,:),v_xr(itr,1)]',[v_yr(itr,:),v_yr(itr,1)]','k-');
    plot([v_xr(itr,1),v_xt(itr)]',[v_yr(itr,1),v_yt(itr)]','k-');

    %FoV
    for iitr=1:10:nt,
        cla
        plot(v_xt,v_yt,'r','LineWidth',3);
        plot(v_xr,v_yr,'k','LineWidth',1);

        plot([v_xr(iitr,:),v_xr(iitr,1)]',[v_yr(iitr,:),v_yr(iitr,1)]','k-');
        plot([v_xr(iitr,1),v_xt(iitr)]',[v_yr(iitr,1),v_yt(iitr)]','k-');
        for ii=1:N, %Draw FoV of all the robots
            colori=colors(ii,:);
            betari= v_fir(iitr,ii)+thetar0(1,ii)-pi+betaMax : -betaMax/10 : v_fir(iitr,ii)+thetar0(1,ii)-pi-betaMax;
            xcirc= m_v_dr(iitr,ii)*cos(betari) + v_xr(iitr,ii);
            ycirc= m_v_dr(iitr,ii)*sin(betari) + v_yr(iitr,ii);

            plot([v_xr(iitr,ii),xcirc,v_xr(iitr,ii)]', [v_yr(iitr,ii),ycirc,v_yr(iitr,ii)]','-','color',colori,'LineWidth',2);

            drawRobots([v_xr(iitr,ii),v_yr(iitr,ii),v_fir(iitr,ii)], 'k-', 1);
        end
        drawRobots([v_xt(iitr),v_yt(iitr),v_fit(iitr)], 'r-', 1);
        plot(v_xt(iitr),v_yt(iitr), '*r');

        axis equal
        pause(0.05)
        title('Evolution of the reference formation. Top view (x-y)');
    end

end %if animate == 1
 

🎉3 参考文献

部分理论源于网络，如有侵权请联系删除。

[1]Gonzalo Lopez-Nicolas, Miguel Aranda, Youcef Mezouar (2019) Adaptive Multirobot Formation to Enclose and Track a Target with Motion and Visibility Constraints

[2]谢兆昆. 基于自适应的多机器人优化编队控制[D].郑州大学,2020.DOI:10.27466/d.cnki.gzzdu.2020.000221.

🌈4 Matlab代码实现