Scribd
Upload
18 views

8th Semester Path PATH LAB

The document summarizes a student project on path planning using random particle optimization. It includes the code and output for RPO to navigate a robot from its starting position to a goal position while avoiding obstacles. The code initializes particle positions around the robot, calculates fitness based on distance to goal and potential fields, and iteratively moves the robot to the highest fitness particle position until it reaches the goal.

Uploaded by

سيد احسن رضا
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
18 views

8th Semester Path PATH LAB

The document summarizes a student project on path planning using random particle optimization. It includes the code and output for RPO to navigate a robot from its starting position to a goal position while avoiding obstacles. The code initializes particle positions around the robot, calculates fitness based on distance to goal and potential fields, and iteratively moves the robot to the highest fitness particle position until it reaches the goal.

Uploaded by

سيد احسن رضا
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 5

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.

You might also like