PATH PLANNING

LAB
RPO – Random Particle Optimization
10 - 12

Submitted by:

Faizan Ali 140627

Hamza Naeem 140624

Submitted to:
Sir Umair Aziz
Code :
%Current Position
currentPos = [2 2];
%Obstacles
obsPos = [3 3;
3 4;
3 5;
4 3;
4 4;
4 5];
%Goal Position
goalPos = [5 5];

hold on; %Graph Begins

rectangle('position', [1 1 5 5]); %Boundary

radius = 0.1; %Radius

N = 30; %Number of Points
[nObs,~] = size(obsPos);
alpha = 5; %Alpha Value
u = 50; %Mean Value
goalMultiplier = 1; %Goal Intensity
obsMultiplier = 1; %Obstacle Intensity

theta = 360/N; %Min Angle

fi = zeros(N,1); %Initializing fi Angles
sum = 0;
%Points Around Current Position Variables
xP = zeros(N,1);
yP = zeros(N,1);
xDP = zeros(N,1); yDP = zeros(N,1);
jot = zeros(N,1);
jgt = zeros(N,1);
jt = zeros(N,1);
errorDist = zeros(N,1);
errorPot = zeros(N,1);
generation = [xP yP errorDist errorPot];
fitness = zeros(N,1);

%Calculating fi Angles
for i = 1:N
sum = sum + theta;
fi(i) = sum;
end

for o = 1:nObs

plot(obsPos(o,1),obsPos(o,2),'o','MarkerEdgeColor','r','Marker
FaceColor','r'); %Obs Position
end
plot(goalPos(1),goalPos(2),'o','MarkerEdgeColor','g','MarkerFa
ceColor','g'); %Goal Position

%Distance of CurrentPos=>Goal
xDGoal = (goalPos(1)-currentPos(1));
yDGoal = (goalPos(2)-currentPos(2));
dGoal = ((xDGoal^2) + (yDGoal^2))^0.5;

while dGoal > 0.1

% xDObs = (obsPos(1)-currentPos(1));
% yDObs = (obsPos(2)-currentPos(2));
% dObs = ((xDObs^2) + (yDObs^2))^0.5;
% radius = 0.4 * dObs;

plot(currentPos(1),currentPos(2),'o','MarkerEdgeColor','b','Ma
rkerFaceColor','b');%Robot Position
pause(0.1);
%Distance of CurrentPos=>Goal
xDGoal = (goalPos(1)-currentPos(1));
yDGoal = (goalPos(2)-currentPos(2));
dGoal = ((xDGoal^2) + (yDGoal^2))^0.5;
%Forces of Attraction and Repulsion of CurrentPos
JOT = alpha*exp(-u*(((currentPos(1)-
obsPos(1))^2)+((currentPos(2)-obsPos(2))^2)));
JGT = -alpha*exp(-u*(((currentPos(1)-
goalPos(1))^2)+((currentPos(2)-goalPos(2))^2)));
JT = JOT * obsMultiplier + JGT * goalMultiplier;

%Generating Points Around Current Position

for p = 1:N
xP(p) = currentPos(1) + radius * cos(pi * fi(p)/180);
yP(p) = currentPos(2) + radius * sin(pi * fi(p)/180);
%plot(xP(p),yP(p),'.','color','b');
%Calculating Cost of Point
%errorDist
xDP(p) = (xP(p)-goalPos(1));
yDP(p) = (yP(p)-goalPos(2));
dP(p) = ((xDP(p)^2)+(yDP(p)^2))^0.5;
errorDist(p) = dP(p)-dGoal;
%errorPotential
errorPot = zeros(N,1);
for o = 1:nObs
jot(p) = alpha*exp(-u*(((xP(p)-
obsPos(o,1))^2)+((yP(p)-obsPos(o,2))^2)));
jgt(p) = -alpha*exp(-u*(((xP(p)-
goalPos(1))^2)+((yP(p)-goalPos(2))^2)));
jt(p) = jot(p) * obsMultiplier + jgt(p) *
goalMultiplier;
errorPot(p) = errorPot(p) + jt(p) - JT;
end
 fitness(p) = 1/((1000+errorDist(p))+
(errorPot(p)*0.5));
end
%Updating Generation
generation = [xP yP fitness]
%Selecting the Best Point
[~,index] = max(generation(:,3));
bestPoint = generation(index,1:2)

%Updating Current Position

currentPos = bestPoint;

end

Output:
Attractive – Repulsive Potentials:

Note:
RPO for non-holonomic (optional) is also done, code and output can be provided upon
request.