% WORM SEARCH 02
%
% Autonomous agents explore a big rectangle area aware only of their personal history and current position.
% Their omnidirectional sensors can (1) look for stuff over a finite radius and (2) detect other vehicles and walls
% that are close by. Otherwise, they have no idea what is going on. The certainty (probability) of a sensed region
% decays as a function of time. The graphics of this decay resemble a "worm". Each vehicle (worm) maintains the
% same velocity. As the agent moves, the radius of its coverage changes. The amount that the surface changes is
% dictated by the matrix 'Radi'. One feature encourages the agent to increase momentum if the radius is decreasing.
% This encourages agents to explore more difficult areas with a greater granularity and thus avoid "looking under
% the lamp post".
%
% Parameters must be entered manually in the Matlab code. Here are the most important parameters.
%
% PARAMETERS
% 1. SP = SPEED of the worm in pixels per iteration,
% 2. r = RADIUS, in pixels, of the sensing field,
% 3. FD = FIELD DECAY = amount of decay per iteration 0 < FD < 1 (Usually, FD is close to 1),
% 4. NX and NY = Dimensions of the box in which the worms live,
% 5. NA = number of agents (worms),
% 6. GEN = The number of iterations to view
%
% The direction of these velocities is controled by a number of disjuntive control factors.
%
% CONTROL FACTORS
% 1. MOMENTUM: A worm continues in the same direction it has traveled unless it hits a wall.
% If it hits a wall, it reflects like light off of a mirror.
% 2. RANDOM: The velocity has a random component.
% -> RV = The relative importance of randomness to momentum. To turn off this property, set RV to zero.
% 3. REPULSION: One worm does not want to bump heads with another.
% -> RP = The relative importance of repulsion to momentum. To turn off this property, set RP to zero.
% -> NRR = Distance, in sensor radii (r), that one worm will sense another worm.
% For distances greater than NNR, one worm will not know anything about another.
% 4. WALL AVOIDANCE: Prohibits worms from hitting walls.
% -> WA = The relative importance of wall avoidance to momentum. To turn off this property, set WA to zero.
% -> NR = Distance from wall in radii (r) that a worm becomes aware of a wall.
% 5. CENTER-OF-MASS AVOIDANCE: Nudges velocity away from the personal center of mass of a worm's historical
% trajectory. This is an idea that sounds good, but doesn't work real well. It drives worms to the wall.
% -> CM = The relative importance of the center of mass to the momentum. To turn off this property, set WA
% to zero.
% 6. RADIUS DIMINISHMENT: When the radius of exploration decreases (increases), the momentum of the agent is
% increased (decreased). This motivates agents to explore difficult areas with a higher granularity and
% thus not "look under the lamp post".
% -> RD = The relative importance of the momentum reduction as a function of radius diminishment as compared
% to the momentum. To turn off this property, set RD = 0.
%
% GRAPHICS
% 1. A movie is made of the worms.
% -> MP = The number of times the movie plays.
% 2. The average coverage (sum of all pixel values over the total area) is in the vector AV.
% It is plotted automatically.
% 3. A smothed version of AV is SAV. It is plotted automatically. Both AV and SAV will resemble a charging
% capacitor - an [ 1 - exp(-t) ] type curve that reaches a noisy steady state.
% -> ED = smoothing factor: 0 < ED < 1. (Usually, ED is close to 1),
% 4. PerCent is the average temporal coverage of each pixel over the entire run. It is plotted automatically.
% Since it takes a while to get to steady state, the average is not evaluated until steady state is achieved.
% -> SS = number of initial steps before the temporal average begins to be computed.
%
close all
clear all
%%%%%%%%%%%%%%%%%%%%%%
% PROBLEM PARAMETERS %
%%%%%%%%%%%%%%%%%%%%%%
NX=125;NY=125; % Size of field
NA=15; % Number of Agents
SP=3; % Speed in pixels per iteration
r=1.; % Sensor Radius
GEN=2500; % Generations = Number of Steps
SS=150; % Number of generations estimated before steady state
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ENVIORNMENTAL PARAMETERS %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
sur=2; % Chooses radius variation surface. 'sur=0' results in no radius variation
FD=0.99; % Field Decay = the rate per iteration of probability decay
%
%%%%%%%%%%%%%%%%%%%%%%
% CONTROL PARAMETERS %
%%%%%%%%%%%%%%%%%%%%%%
%
% RANDOM PARAMETER
RV=.3; % Randomness importance of velocity between 0 1nd 1
%
% REPULSION PARAMETERS
RP=3.; % REPULSION importance twixt 0 and 1
NRR=3.5;%5; % Number of radii for repulsion
%
% WALL AVOIDANCE PARAMETERS
WA=0.5; % Strength of WALL Avoidance
NR=1; % Number of Radii from wall where repulsion becomes effective
%
% CENTER OF MASS AVOIDANCE PARAMETER
CM=0; %Importance of avoidance of center of mass
% % Note: This property has been found to not work well. Set to zero.
RD=0.025; %Importance of radius diminishment
%
%%%%%%%%%%%%%%%%%%%%%%%
% GRAPHICS PARAMETERS %
%%%%%%%%%%%%%%%%%%%%%%%
%
MP=0; % Number of times the movie plays
ED=0.95; % Error Decay
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RADIUS VARIATION SURFACE %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
POS=zeros(2,NA);
POS(1,:)=ceil(NX*rand(size(POS(1,:))));POS(2,:)=ceil(NY*rand(size(POS(2,:)))); % Location of agents
Radi=ones(NX,NY);
if(sur==0)
elseif(sur~=0)
xxx=linspace(1,NX); yyy=linspace(1,NY);
if(or(sur==1,sur==2)) % Gaussian: means mx,my. Standard deviations varx,vary. Correlation rh
mx=floor(NX/2); my=floor(NY/3);
varx=100; vary=100; rh=.6;
for xx=1:NX
for yy=1:NY
Radi(xx,yy)=exp(((-(xx-mx)^2+2*rh*(xx-mx)*(yy-my)-(yy-my)^2))/(varx*vary));
end
end
end
Radi=Radi/min(min(Radi));
Radi=Radi.^1.5;
if(sur==2)
Radi=ones(size(Radi))*(1+max(max(Radi)))-Radi;
end
imagesc(Radi);colorbar;
figure
end
%
%%%%%%%%%%%%%%%%%%
% INITIALIZATION %
%%%%%%%%%%%%%%%%%%
%
AV=zeros(GEN,1);SAV=AV;
r2=r*r;
oox=ones(size(POS(1,:)));
ooy=ones(size(POS(2,:)));
Playground=zeros(NX,NY);
PerCent=Playground;
onez=ones(size(PerCent));
if(CM>0)
PLAYagent=zeros(NX,NY,NA);
end
VEL=ones(2,NA);
v1=rand(2,NA);v2=sqrt(v1(1,:).*v1(1,:)+v1(2,:).*v1(2,:));
mmm=ceil(rand(2,NA)-.5*VEL);
VEL(1,:)=((-1).^mmm(1,:)).*round(SP*v1(1,:)./v2);
VEL(2,:)=((-1).^mmm(1,:)).*round(SP*v1(2,:)./v2); % Velocity
xxx=linspace(1,NX,NX);
yyy=linspace(1,NY,NY);
VELcom=zeros(2,NA);
VELdim=zeros(2,NA);
%
%%%%%%%%%%%%%%%%%%%
% GENERATION LOOP %
%%%%%%%%%%%%%%%%%%%
for g=1:GEN
den=sqrt(VEL(1,:).*VEL(1,:)+VEL(2,:).*VEL(2,:))+ones(size(VEL(2,1)));
VEL(1,:)= round(SP*VEL(1,:)./den);VEL(2,:)= round( SP*VEL(2,:)./den);
POSold=POS;
POS=POS+VEL;
XX=repmat(POS(1,:),NA,1); DX=XX-XX';
YY=repmat(POS(2,:),NA,1); DY=YY-YY';
DD=DX.*DX+DY.*DY; % Matrix of square of distance between agents
% Update Field
Playground=FD*Playground;
if(g>=SS)
PerCent=PerCent+Playground;
end
if(CM>0)
PLAYagent=FD*PLAYagent;
end
for a=1:NA
p1=max(min(POS(1,a),NX),1);
p2=max(min(POS(2,a),NY),1);
rrr=r*Radi(p1,p2);
rrr2=rrr*rrr;
rrrr=round(rrr);
for x=(POS(1,a)-rrrr):(POS(1,a)+rrrr);
if(and(x>0,x<=NX))
for y=(POS(2,a)-rrrr):(POS(2,a)+rrrr);
if(and(y>0,y<=NY))
xr=x-POS(1,a);yr=y-POS(2,a);
if(xr^2+yr^2<=rrr2)
Playground(x,y)=1;
if(CM>0)
PLAYagent(x,y,a)=1;
end
end
end
end
end
end
end
for a=1:NA
if(or(POS(1,a)<1,POS(1,a)>NX))
VEL(1,a)=-VEL(1,a);
if(POS(1,a)<1);POS(1,a)=1;end; if(POS(1,a)>NX);POS(1,a)=NX;end
end
if(or(POS(2,a)<1,POS(2,a)>NY))
VEL(2,a)=-VEL(2,a);
if(POS(2,a)<1);POS(2,a)=1;end; if(POS(2,a)>NY);POS(2,a)=NY;end
end
end
%%%%%%%%%%%%
% REPULSION
VELrepulsion=zeros(2,NA);
DDPLUS=DD+NX*NY*eye(size(DD));
[neigh,argneigh]=min(DDPLUS);
for a=1:NA
NNRR= NRR*Radi(POS(1,a),POS(2,a));
if(neigh(a)<=NNRR*NNRR*r2)
VELrepulsion(1,a)=-DX(a,argneigh(a));
VELrepulsion(2,a)=-DY(a,argneigh(a));
di=VELrepulsion(1,a)*VELrepulsion(1,a)+VELrepulsion(2,a)*VELrepulsion(2,a)+1;
VELrepulsion(1,a)=SP*VELrepulsion(1,a)/di;
VELrepulsion(2,a)=SP*VELrepulsion(2,a)/di;
end
end
%%%%%%%%%%%%%%%%%
% WALL AVOIDANCE
VELwall=zeros(2,NA);
DWallx=oox+NX*oox/2-abs(POS(1,:)-NX*oox/2);
for a=1:NA
rrr=r*Radi(POS(1,a),POS(2,a));
rrr2=rrr*rrr;
if(DWallx(a)<NR*rrr)
VELwall(1,a)=-SP*sign(POS(1,a)-NX/2)./DWallx(a);
end
end
DWally=ooy+NY*ooy/2-abs(POS(2,:)-NY*ooy/2);
for a=1:NA
rrr=r*Radi(POS(1,a),POS(2,a));
rrr2=rrr*rrr;
if(DWally(a)<NR*rrr)
VELwall(2,a)=-SP*sign(POS(2,a)-NY/2)./DWally(a);
end
end
%%%%%%%%%%%%%%%%%%%%%
%Backtrack Avoidance
if(CM>0)
for a=1:NA
COM=PLAYagent(:,:,a);
cx=sum(COM');cy=sum(COM);
COMa=sum(cx);
comy=sum(yyy.*cy)/COMa;
comx=sum(xxx.*cx)/COMa;
VELcom(1,a)= comx-POS(1,a);
VELcom(2,a)= comy-POS(2,a);
VELcomDEN=sqrt(VELcom(1,a)*VELcom(1,a)+VELcom(2,a)*VELcom(2,a))+1;
VELcom(:,a)= -VELcom(:,a)/VELcomDEN;
end
end
%%%%%%%%%%%%%%%%%%%
% Radi Dimininish
if(sur~=0)
for a=1:NA
p1=max(min(POS(1,a),NX),1);
p2=max(min(POS(2,a),NY),1);
p1old=max(min(POSold(1,a),NX),1);
p2old=max(min(POSold(2,a),NY),1);
RADIdiff=Radi(p1,p2)-Radi(p1old,p2old);
VELdim(1,a)= -sign(RADIdiff)*SP;
VELdim(2,a)= -sign(RADIdiff)*SP;
end
end
AV(g)=sum(sum(Playground))/(NX*NY);
if(g>1)
SAV(g)=(1-ED)*AV(g)+ED*SAV(g-1);
end
%
%%%%%%%%%%%%
% GRAPHICS %
%%%%%%%%%%%%
imagesc(Playground);hold on;
title(g); hold on;
%plot(NX,NY,'w+'); hold on
for a=1:NA
plot(POS(2,a),POS(1,a),'w.'); hold on;
end
M(g)=getframe;
hold on;
hold off;
% Random velocity
VELrandom=RV*SP*(2*rand(size(VEL))-ones(size(VEL)));
% Velocity Aggregation
VEL=VEL+RV*VELrandom+RP*VELrepulsion+WA*VELwall+CM*VELcom+RD*VELdim.*VEL;
end
%
%%%%%%%%%%%%%%%%%%
% FINAL GRAPHICS %
%%%%%%%%%%%%%%%%%%
figure
plot(AV);title('Average Coverage');
figure
plot(SAV);title('Smoothed');xlabel(SAV(GEN));
figure
PerCent(NX,NY)=GEN-SS;
PerCent(1,1)=0;
imagesc(PerCent/(GEN-SS));colorbar;title('Per Cent Coverage');
if(MP>0)
figure
movie(M,MP)
end
