💥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.
