%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Vehicle guidance for obstacle avoidance example
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kevin Passino
% Version: 1/25/01
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear % Initialize memory
xmin=[0; 0]; % Set edges of region want to search in
xmax=[30;40];
Nsteps=500; % Maximum number of steps to produce
% Next set the parameters of the vehicle:
lambda=0.1; % Step size to take in chosen direction at each move
Ns=16; % Number of points on circular pattern to sense
r=1; % Sensing radius
xs=0*ones(2,Ns); % Initialize
Jo(:,1)=0*ones(Ns,1);
Jg(:,1)=0*ones(Ns,1);
J(:,1)=0*ones(Ns,1);
theta(:,1)=0*ones(Ns,1);
for m=2:Ns % Compute the angles to be used around the circle
theta(m,1)=theta(m-1,1)+(pi/180)*(360/Ns);
end
% Goal position of vehicle
xgoal=[25; 25];
% Initial vehicle position
x=[5; 5];
% Weighting parameters for planning (sets priority for being aggresive
% in the direction of the goal vs. avoiding obstacles
w1=1;
w2=1.0000e-04;
% Allocate memory
x(:,2:Nsteps)=0*ones(2,Nsteps-1);
% The obstacles:
figure(1)
clf
% Plot initial and final positions
plot(5,5,'s',25,25,'x')
axis([0 30 0 30])
hold on
xlabel('x');
ylabel('y');
title('Obstacles (o), initial vehicle (square) and goal (x) positions');
hold on
% Plot obstacle positions (sets obstaclefunction)
plot(20,15,'o',8,10,'o',10,10,'o',12,10,'o',24,20,'o',18,20,'o')
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the functions:
xx=0:31/100:30; % For our function the range of values we are considering
yy=xx;
% Compute the obstacle and goal functions
for jj=1:length(xx)
for ii=1:length(yy)
zz(ii,jj)=obstaclefunction([xx(jj);yy(ii)],w1);
end
end
for jj=1:length(xx)
for ii=1:length(yy)
zzz(ii,jj)=goalfunction([xx(jj);yy(ii)],xgoal,w2);
end
end
figure(2)
clf
surf(xx,yy,zz);
%colormap(jet)
% Use next line for generating plots to put in black and white documents.
colormap(white);
xlabel('x');
ylabel('y');
zlabel('w_1J_o');
title('Function w_1J_o showing (scaled) obstacle function values');
figure(3)
clf
contour(xx,yy,zz,25)
colormap(jet)
% Use next line for generating plots to put in black and white documents.
%colormap(white);
xlabel('x');
ylabel('y');
title('Contour map of w_1J_o and initial (square) and goal (x) positions');
hold on
% Plot initial and final positions
plot(5,5,'s',25,25,'x')
hold off
figure(4)
clf
surf(xx,yy,zzz);
view(82,26);
%colormap(jet)
% Use next line for generating plots to put in black and white documents.
colormap(white);
xlabel('x');
ylabel('y');
zlabel('w_2J_g');
title('Goal function (scaled)');
%rotate3d
figure(5)
clf
contour(xx,yy,zzz,25)
colormap(jet)
% Use next line for generating plots to put in black and white documents.
%colormap(gray);
xlabel('x');
ylabel('y');
title('Contour function of w_2J_g and initial (square) and goal (x) positions');
hold on
% Plot initial and final positions
plot(5,5,'s',25,25,'x')
hold off
figure(6)
clf
contour(xx,yy,zz+zzz,50)
colormap(jet)
% Use next line for generating plots to put in black and white documents.
%colormap(gray);
xlabel('x');
ylabel('y');
title('J=w_1J_o + w_2J_g and initial (square) and goal (x) positions');
hold on
% Plot initial and final positions
plot(5,5,'s',25,25,'x')
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the simulation loop
for k=1:Nsteps
% Use projection to keep in boundaries (like hitting a wall and staying at it)
x(:,k)=min(x(:,k),xmax);
x(:,k)=max(x(:,k),xmin);
% Sense points on circular pattern
for m=1:Ns
xs(:,m)=[x(1,k)+r*cos(theta(m,1)); x(2,k)+r*sin(theta(m,1))]; % Point on circular pattern
Jo(m,1)=obstaclefunction(xs(:,m),w1); % Compute the obstace function (what is
% sensed at each sensed point
Jg(m,1)=goalfunction(xs(:,m),xgoal,w2); % Compute how well each point
% moves toward the goal
J(m,1)=Jo(m,1)+Jg(m,1); % Compute function for opt. in planning
end
% Next pick the best direction
[val,bestone]=min(J);
% Then, update the vehicle position (pick best direction and move step of lambda that way)
x(:,k+1)=[x(1,k)+lambda*cos(theta(bestone,1)); x(2,k)+lambda*sin(theta(bestone,1))];
% But the vehicle is in a real environment so when it tries to move to that point it
% only gets to near that point. To simulate this we perterb the final position.
Deltalambda=0.1*lambda*(2*rand-1); % Set the length perturbation to be up to 10% of the step size
Deltatheta=2*pi*(2*rand-1); % Set to be 360deg variation from chosen direction
x(:,k+1)=[x(1,k+1)+Deltalambda*cos(theta(bestone,1)+Deltatheta); ...
x(2,k+1)+Deltalambda*sin(theta(bestone,1)+Deltatheta)];
end % End main loop...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, provide some plots of the results of the simulation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=0:Nsteps; % For use in plotting
figure(7)
clf
plot(t,x(1,:),'k-',t,x(2,:),'k--')
ylabel('x, y')
xlabel('Iteration, k')
title('Vehicle trajectory (x solid, y dashed)')
figure(8)
clf
contour(xx,yy,zz,25)
% Use next line for generating plots to put in black and white documents.
%colormap(gray);
xlabel('x');
ylabel('y');
title('Vehicle path to avoid obstacles and reach goal');
hold on
plot(x(1,:),x(2,:),'o')
plot(5,5,'s',25,25,'x')
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
