% WORM SEARCH 01
%
% 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.
%
% 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.
%
% 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),
%
close all
clear all
%
%%%%%%%%%%%%%%
% PARAMETERS %
%%%%%%%%%%%%%%
%
NX=200;NY=200; % Size of the rectangular field
NA=35; % Number of WORMS (Agents)
SP=3; % Worm speed in pixels per iteration
FD=0.99; % Field Decay - the rate at which probablity decays for each iteration.
r=7.; % Sensor Radius - The omnidirectional sensor range in pixels.
%
%
%%%%%%%%%%%%%%%%%%%
% CONTROL FACTORS %
%%%%%%%%%%%%%%%%%%%
%
% RANDOM
RV=.5; % Randomness importance of velocity between 0 1nd 1
% REPULSION
RP=2; % REPULSION importance twixt 0 and 1
NRR=3.5; % Number of radii for repulsion
% WALL AVOIDANCE
WA=1; % Strength of WALL Avoidance
NR=1.33; % Number of Radii from wall where repulsion becomes effective
% CENTER-OF-MASS AVOIDANCE
CM=0; % Importance of avoidance of center of mass
% GENERATIONS
GEN=400; % Generations = Number of Iterations
%
%%%%%%%%%%%%%%%%%%%%%%
% GRAPHICS PARAMETER %
%%%%%%%%%%%%%%%%%%%%%%
%
MP=3; % Number of times to play the worm movie
ED=0.95; % Error Decay - a parameter for smoothing the coverage curve
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MAKE NO CHANGES BELOW THIS LINE %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% INITIALIZATION
AV=zeros(GEN,1);SAV=AV;
r2=r*r;
POS=zeros(2,NA);
POS(1,:)=ceil(NX*rand(size(POS(1,:))));POS(2,:)=ceil(NY*rand(size(POS(2,:)))); % Location of agents
oox=ones(size(POS(1,:)));
ooy=ones(size(POS(2,:)));
Playground=zeros(NX,NY);
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);
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);
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(CM>0)
PLAYagent=FD*PLAYagent;
end
for a=1:NA
for x=(POS(1,a)-r):(POS(1,a)+r);
if(and(x>0,x<=NX))
for y=(POS(2,a)-r):(POS(2,a)+r);
if(and(y>0,y<=NY))
xr=x-POS(1,a);yr=y-POS(2,a);
if(xr^2+yr^2<=r2)
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
if(neigh(a)<=NRR*NRR*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
if(DWallx(a)<NR*r)
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
if(DWally(a)<NR*r)
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
AV(g)=sum(sum(Playground))/(NX*NY);
if(g>1)
SAV(g)=(1-ED)*AV(g)+ED*SAV(g-1);
end
% IMAGING
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;
end
figure
plot(AV);title('Average Coverage');
figure
plot(SAV);title('Smoothed');xlabel(SAV(GEN));
if(MP>0)
figure
movie(M,MP)
end
