% c1.m
% circle forming
.
.
gs1=3;
gs2=10000;
.
.
for t=1:nt,
t
for i=1:nrob,
distg=norm(goal-rob(:,i));
gvec(:,i)=goal-rob(:,i);
goalvec(:,i)=(goal-rob(:,i))/distg; % vector to goal
end
for i=1:nrob,
if norm(gvec(:,i))>d+eps,
gs=gs1+(norm(gvec(:,i))^2)/gs2;
rr=0;rtrv(:,i)=[0;0];minx=1;
elseif norm(gvec(:,i))
A.2 Algorithm Fill-Circle
% c2.m
% algorithm fill-circle
.
.
gs1=3;
gs2=10000;
.
.
for t=1:nt,
t
for i=1:nrob,
distg=norm(goal-rob(:,i));
gvec(:,i)=goal-rob(:,i);
goalvec(:,i)=(goal-rob(:,i))/distg; % vector to goal
end
for i=1:nrob,
if norm(gvec(:,i))>d,
gs=gs1+(norm(gvec(:,i))^2)/gs2;
rr=0;rtrv(:,i)=[0;0];minx=1;
else
for j=1:nrob,
if j~=i,
rtrd(j)=norm(rob(:,i)-rob(:,j));
rtrv(:,j)=(rob(:,i)-rob(:,j))/rtrd(j);
end
end
rtrd(i)=100;
[clst,minx]=min(rtrd);
gs=0; rr=2;
end
nxrob(:,i)=rob(:,i)+gs*goalvec(:,i)+rr*rtrv(:,minx);
end
.
.
rob=nxrob;
end %t
.
.
A.3 Algorithm Form-Sphere
% sphere.m
% sphere forming
.
.
eps=3;
gs1=4;
gs2=10000;
.
.
for t=1:nt,
t
for i=1:nrob,
distg=norm(goal-rob(:,i));
gvec(:,i)=goal-rob(:,i);
goalvec(:,i)=(goal-rob(:,i))/distg; % vector to goal
end
for i=1:nrob,
if norm(gvec(:,i))>d+eps,
gs=gs1+(norm(gvec(:,i))^2)/gs2;
rr=0;rtrv(:,i)=[0;0;0];minx=1;
elseif norm(gvec(:,i))
A.4 Algorithm Form-Paraboloid
% program paraboloid.m
.
.
ep=1;
r1=ceil(rand*nrobot);
r2=r1;
while r2==r1,
r2=ceil(rand*nrobot);
end
p2=[500;500;150]; % final positions for r1 and r2
p1=[500;500;850]; % (vector p1-p2 must be // to z-axis)
a=norm(p1-p2); % assumed to be fed to robots
b=300; % neig.d of r2 : fed to robots
for i=1:nrobot,
dp1(i)=norm(robot(:,i)-p1);
dp2(i)=norm(robot(:,i)-p2);
end
[xx,r1]=min(dp1);
[xx,r2]=min(dp2);
.
.
% user interface 'Go'
subr=['p51'];
goc=uicontrol('style','push','units','normal','string','Go',
'pos',[.9 .7 .08 .07],'call',subr);
---
% program p51.mcallback subroutine of GOC in paraboloid.m
for t=1:ntime,
t
nxrobot(:,r1)=robot(:,r1)+(1+norm(p1-robot(:,r1))^2/3000)*
(p1-robot(:,r1))/norm(p1-robot(:,r1));
nxrobot(:,r2)=robot(:,r2)+(1+norm(p2-robot(:,r2))^2/3000)*
(p2-robot(:,r2))/norm(p2-robot(:,r2));
for i=1:nrobot,
if i~=r1 & i~=r2,
v1=nxrobot(:,r1)-robot(:,i);d1=norm(v1);
v2=nxrobot(:,r2)-robot(:,i);d2=norm(v2);
z=(a^2-d1^2+d2^2)/(2*a);
x2y2=(d2^2-z^2);
if d2>d1,
nxrobot(:,i)=robot(:,i)+(1+d2^2/8000)*v2/d2;
% move twd r2
elseif d2>b,
nxrobot(:,i)=robot(:,i)+(1+d2^2/8000)*v2/d2
% move twd r2
elseif z>x2y2/(4*a)+ep,
nxrobot(:,i)=robot(:,i)-(1+(d1-a)^2/5000)*v1/d1+2*v2/d2;
% move away from r1 + to r2
elseif zep | norm(robot(:,i)-p2)> b & i~=r1,
set(plr(i),'xdata',robot(1,i),'ydata',robot(2,i),
'zdata',robot(3,i),'vis','on');
end
end
A.5 Algorithm Switching-Distance
% ds.m
% p1 (matlab 4.0 program 1 switching ds)
% call subroutines: p11.m, p12.m p13.m
.
.
global subr
.
.
ngoal=3; % number of goals
nrobot=20; % number of ronots
eyea=1000; % detection region
ntime=50; % final time.
dn=20; % neighborhood
rtrr=2; % repulsion vector magnitude
gs1=5;
gs2=1500;
formul='gs1+DTG^2/gs2'; % displacement formula for region d>dn
.
.
goc=uicontrol('style','push','units','normal','pos',[0.03,.19,.20,.06]
,'string','Sim','call',subr);
.
.
for c=1:ngoal+1, % initial goal configuration
gvt(1,c)=0;
for cc=1:nrobot,
if gv(cc)==c,
gvt(1,c)=gvt(1,c)+1;
end
end
end
.
.
for t=1:ntime, % time loop
ds=findtime(sdfunc,t,deg);
set(f1p3,'xdata',t,'ydata',ds)
---
% program p11.m
% callback subroutine of user interface GOC in p1 (ds.m)
.
.
for i=1:nrobot, %signal assignments
for k=1:ngoal,
distg(k)=norm(goal(:,k)-robot(:,i));
end
[clog,ind]=min(distg);
if clog<=eyea,
gv(i)=ind;
else
gv(i)=ngoal+1; % if robot can't see any goal, assign
end % to virtual goal at [3000,3000]
end
for i=1:nrobot,
distg=norm(goal(:,gv(i))-robot(:,i));
gvec(:,i)=goal(:,gv(i))-robot(:,i);
goalvec(:,i)=(goal(:,gv(i))-robot(:,i))/distg;
% vector to closest goal
if norm(gvec(:,i))<=dn, % if in dn, signal
signal(i)=gv(i);
else % if not, do not signal
signal(i)=0;
end
end
for i=1:nrobot,
if norm(gvec(:,i))>1500, % no goal in 'eye', then move random
rr=0;rtrvec(:,i)=[0;0];
goalvec(:,i)=-1+2*rand(2,1);gs=3; % can be changed
elseif norm(gvec(:,i))>dn, % go twd goal
DTG=norm(gvec(:,i));
gs=eval(formul); % formula for gs
rr=0;rtrvec(:,i)=[0;0];
else
mates=[0;0];rtrd=0;rtrvec(:,i)=[0;0];tt=1;
for j=1:nrobot,
if signal(j)==signal(i) & j~=i,
mates(:,tt)=robot(:,j);
tt=tt+1;
end
end
% teammates in d known
if mates(:,1)==[0;0], % no mates
rtrvec(:,i)=[0;0];
else
for j=1:tt-1, % there are some mates around
rtrd(j)=norm(mates(:,j)-robot(:,i));
end
[dummy,minx]=min(rtrd);
rtrvec(:,i)=(mates(:,minx)-robot(:,i))/rtrd(minx);
end
% move away from closest teammate
rr=rtrr;gs=0;
end
nxtrob(:,i)=robot(:,i)+gs*goalvec(:,i)-rr*rtrvec(:,i);
% change goal if inside ds
if (norm(nxtrob(:,i)-goal(:,gv(i)))<=ds & ds>dn) |
(norm(nxtrob(:,i)-goal(:,gv(i)))>1500),
tr=1;
gv(i)=ceil(rand*ngoal);
while tr<=20 & norm(nxtrob(:,i)-goal(:,gv(i)))>eyea,
gv(i)=ceil(rand*ngoal);
tr=tr+1; % ideal case: tr=very large
end
if norm(nxtrob(:,i)-goal(:,gv(i)))>eyea,
%if still no goal "around"
gv(i)=ngoal+1;
end
st=t;
end
end
robot=nxtrob;
.
.
end % time loop ends
.
.
A.6 Algorithm Family Values
% program p3 (famval.m)
% main program for simulation famval
.
.
global subr
.
.
% initial values for parameters
ngoal=3;
nrobot=20;
ntime=30;
.
.
dn=20;
fv=[1:nrobot 0];
first=zeros(1,nrobot); % vector for "first" def.
gs1=4;
gs2=1500;
rtrr=2;
eyer=1500; % robot dedection region of all agents
eyeg=1500; % goal dedection region of all agents
formul='gs1+DTG^2/gs2'; % displacement formula for r>dn
.
.
goc=uicontrol('style','push','units','normal','pos',[0.03,.22,.20,.06]
,'string','Sim','call',subr);
.
.
---
% program p31.m
% callback subroutine for user interface GOC in famval.m
.
.
for i=1:nrobot, %signal assignments
for k=1:ngoal,
distg(k)=norm(goal(:,k)-robot(:,i));
end
[clog,ind]=min(distg);
if clog<=eyeg,
gv(i)=ind;
else
gv(i)=ngoal+1 % if robot can't see any goal assign
end % virtual goal at [3000,3000]
end
for c=1:ngoal+1, % initial goal configuration
gvt(1,c)=0;
for cc=1:nrobot,
if gv(cc)==c,
gvt(1,c)=gvt(1,c)+1;
end
end
end
.
.
for t=1:ntime,
set(f1p2,'xdata',[-.6 -.6+1.2*t/ntime])
for i=1:nrobot,
distg=norm(goal(:,gv(i))-robot(:,i));
gvec(:,i)=goal(:,gv(i))-robot(:,i);
goalvec(:,i)=(goal(:,gv(i))-robot(:,i))/distg;
% vector to closest goal
if norm(gvec(:,i))<=dn, % if in dn, signal
signal(i)=gv(i);
if first(i)==0 & length(find(first==gv(i)))~=1,
%if you're the first one in neig.d
first(i)=gv(i);
end
else % if not, do not signal
signal(i)=0;
end
end
fx=find(first>0); % fam.val. shifting
for i=1:length(fx),
numseen(i)=0;
for j=1:length(fx),
if j~=i & norm(robot(:,fx(i))-robot(:,fx(j)))nrobot/2,
dist(i,j)=abs(fv(fx(i))-nrobot-fv(fx(j)));
end
if dist(i,j)>nrobot/2,
dist(i,j)=abs(fv(fx(i))+nrobot-fv(fx(j)));
end
else
dist(i,j)=100;
end
end
[clst,clstx]=min(dist(i,:));
if clst==100,
nfv(fx(i))=fv(fx(i));
elseif fv(fx(i))>fv(fx(clstx)),
if abs(fv(fx(i))-fv(fx(clstx)))>nrobot/2,
nfv(fx(i))=fv(fx(i))-.5;
else
nfv(fx(i))=fv(fx(i))+.5;
end
else
if fv(fx(i))==fv(fx(clstx)),
nfv(fx(i))=fv(fx(i))-.5+round(rand);
elseif abs(fv(fx(i))-fv(fx(clstx)))>nrobot/2,
nfv(fx(i))=fv(fx(i))+.5;
else
nfv(fx(i))=fv(fx(i))-.5;
end
end
if nfv(fx(i))>nrobot,
nfv(fx(i))=nfv(fx(i))-nrobot;
end
if nfv(fx(i))<0.5,
nfv(fx(i))=nfv(fx(i))+nrobot;
end
fam(fx(i))=nrobot/(numseen(i)+1);
end
if exist('nfv')>0,
fv(fx)=nfv(fx);
end
.
.
for i=1:nrobot, % next locations
if norm(gvec(:,i))>1500, % if no goal in 'eye', then move random
rr=0;rtrvec(:,i)=[0;0];
goalvec(:,i)=-1+2*rand(2,1);gs=4; % can be changed
elseif norm(gvec(:,i))>dn, % go twd goal
gs=gs1+(norm(gvec(:,i)))^2/gs2;
rr=0;rtrvec(:,i)=[0;0];
else
mates=[0;0];rtrd=0;rtrvec(:,i)=[0;0];tt=1;
for j=1:nrobot,
if signal(j)==signal(i) & j~=i,
mates(:,tt)=robot(:,j);
tt=tt+1;
end
end
% teammates in d known
if mates(:,1)==[0;0], % no mates
rtrvec(:,i)=[0;0];
else
for j=1:tt-1, % there are some mates around
rtrd(j)=norm(mates(:,j)-robot(:,i));
end
[dummy,minx]=min(rtrd);
rtrvec(:,i)=(mates(:,minx)-robot(:,i))/rtrd(minx);
end
% move away from closest teammate
rr=rtrr;gs=0;
end
nxtrob(:,i)=robot(:,i)+gs*goalvec(:,i)-rr*rtrvec(:,i);
% change goal if (first's fv-yours)>fam/2
ix=find(first==gv(i));
if length(ix)==0,
ix=nrobot+1;fam(ix)=10000;
end
if norm(gvec(:,i))<=eyer & first(i)==0 &
abs(fv(i)-fv(ix))>(.5+fam(ix))/2 &
((abs(fv(i)-fv(ix)+nrobot)>(.5+fam(ix))/2 & fv(i)(.5+fam(ix))/2 & fv(i)>fv(ix))),
tr=1;
gv(i)=ceil(rand*ngoal);
while tr<=20 & norm(nxtrob(:,i)-goal(:,gv(i)))>eyeg,
gv(i)=ceil(rand*ngoal);
tr=tr+1; % ideal case: tr=very large
end
if norm(nxtrob(:,i)-goal(:,gv(i)))>eyeg, %if no goal "around"
gv(i)=ngoal+1;
end
st=t;
end
end
.
.
robot=nxtrob;
end % time loop
.
.
A.7 Algorithm Find-pallet
% finding pallets in the warehouse
% simulation for beacon,robot interactions and possible
% strategies for finding pallets
% general algorithm:
% 1. if see pallet, move twd. pallet
% 2. if see no pallet but another agent going to destination,
% follow that agent
% 3. if no pallet nor agent, follow wall
.
.
global obsm
% robot and goal properties
nrobot=10;
ngoal=1;
neig=60;
eyeg=300;
eyer=1200;
th=rand(1,nrobot)*pi;
dir=[cos(th);sin(th)];
step=30;
angstep=36; % i.e. theta=180/18=10
dirch=zeros(1,nrobot);
maxdirch=5;
robt=zeros(4,nrobot);
ntime=300;
% detection vectors
seegr=zeros(2,nrobot);
% =max if goal seen, <=max-1 if another agent seen
% row1= status row2=ID of agent seen
seegrt=seegr;
glev=20;
.
.
for t=1:ntime,
t
seegr
% set signals according to dedection status
seegr=zeros(2,nrobot);
for i=1:nrobot,
if all([norm(robot(:,i)-goal)<=eyeg see(robot(:,i),goal)==1])==1,
seegr(1,i)=glev;
end
end
for lev=glev:-1:glev-nrobot+1,
for i=1:nrobot,
if seegr(1,i)==0, % if no dedection yet
maxx=2000;
for j=1:nrobot,
if all([norm(robot(:,i)-robot(:,j))<=min(eyer,maxx)
see(robot(:,i),robot(:,j))==1 i~=j seegr(1,j)==lev+1])==1,
seegr(1,i)=lev;seegr(2,i)=j;maxx=norm(robot(:,i)-
robot(:,j))
end
end
end
end
end
% here we know all agents' dedection status
% set direction depending on the dedection
for i=1:nrobot,
if seegr(1,i)==glev,
dir(:,i)=-(robot(:,i)-goal)/norm(robot(:,i)-goal);
% to goal if seen
dirch(i)=0;
else
if seegr(1,i)~=0,
dir(:,i)=-(robot(:,i)-robot(:,seegr(2,i)))/norm(robot(:,i)-
robot(:,seegr(2,i)));
dirch(i)=0;
end
end
if dirch(i)>=maxdirch,
left=robot(:,i)+100*[0 -1;1 0]*dir(:,i);
right=robot(:,i)+100*[0 1;-1 0]*dir(:,i);
if min(left)>0 & max(left)<1200,
if obsm(left(1),left(2))==0
dir(:,i)=[0 -1;1 0]*dir(:,i); dirch(i)=0;
end
elseif min(right)>0 & max(right)<1200,
if obsm(right(1),right(2))==0,
dir(:,i)=[0 1;-1 0]*dir(:,i); dirch(i)=0;
end
end
end
end %i
% move in the direction computed above (adjust direction if necessary)
for i=1:nrobot,
if norm(robot(:,i)-goal)>neig,
newp=round(robot(:,i)+step*dir(:,i));
if (max(newp)>1200 | min(newp)<1),
newp=[1 ;1];
end
if see(robot(:,i),newp)==0,
newp=[1 ;1];
end
turn=1;
tpos=robt(size(robt,1)-5:size(robt,1),i);
while obsm(newp(1),newp(2))==1 ,
%if obstacle dedected at step range, change direction
th(i)=th(i)+turn*sign(isodd(turn)-.5)*pi/angstep;
%scan +/- starting with ccw (+angstep)
dir(:,i)=[cos(th(i));sin(th(i))];
newp=ceil(robot(:,i)+step*dir(:,i));
if (max(newp)>1200 | min(newp)<1),
newp=[1; 1];
end
if see(robot(:,i),newp)==0,
newp=[1; 1];
end
turn=turn+1;
end
if ((tpos(1)-tpos(5))^2+(tpos(2)-tpos(6))^2)^.5<=step*sqrt(3),
% 60 or more turn
dirch(i)=dirch(i)+1;
end
robot(:,i)=newp;
end
end %i
.
.
robt=[robt;robot];
end %time
.
.
B. Matlab/Simulink Simulation Snapshots
Screen snapshots of the following Mathworks' Matlab 4.0 and Simulink 2.0 simulations are given here:
- * Matlab program for formation of a sphere.
- Figure B.1 Plot screen of the Matlab program Sphere
- * Matlab program for formation of a paraboloid.
- Figure B.2 Plot screen of the Matlab program Paraboloid
- * Modified VDP Oscillator block.
- Figure B.3 Unmasked modified VDP oscillator block with time varying feedback gain
- * Simulation window for coupled Modified VDP Oscillators.
- Figure B.4 Simulink window for coupled Modified VDP Oscillators
- * Matlab program for multigoal distribution using switching distance.
- Figure B.5 Screen snapshot of the Matlab program ds
- * Matlab program for multigoal distribution using family values.
- Figure B.6 Screen snapshot of the Matlab program famval
Back to Thesis Main Page
To Chapter 6
To Bibliography
