Scribd
Upload
4 views5 pages

Path Planning with Random Particle Optimization

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
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
4 views5 pages

Path Planning with Random Particle Optimization

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
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd

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.

Common questions

Powered by AI

The 'alpha' parameter in the path planning model signifies the intensity of the potential functions used for attraction and repulsion forces. This parameter essentially scales the influence of obstacles and goals, adjusting the sensitivity of the algorithm to changes in potential, thereby affecting the path smoothing and the avoidance behavior towards obstacles .

The algorithm balances goal-oriented navigation with obstacle avoidance by calculating and combining potentials from the goal and obstacles. The goal multiplier attracts the path towards the desired endpoint, while the obstacle multiplier ensures repulsion from obstacles. The resultant potential guides the path, ensuring a smooth and efficient trajectory that safely navigates the environment, resolving conflicts between immediate obstacle avoidance and overall path progress .

In the optimization process, 'jot' represents the repulsive potential from obstacles, calculated as a positive force, while 'jgt' represents the attractive force towards the goal as a negative potential. These potentials form part of the total potential 'jt,' influencing the selection of points that guide the path generation, balancing the path towards minimizing obstacles and reaching the goal .

The fitness matrix functions as an evaluative tool, assigning a fitness score to each potential point based on its error distance from the goal and its error potential with respect to obstacles. This score helps determine the suitability of each point for continuation on the path, allowing the algorithm to choose the most optimal point for the robot to move towards next .

The Mean Value 'u' affects the sharpness or spread of the exponential functions used in both the attractive and repulsive potentials. A higher 'u' sharpens the decline of potential with distance, making the system sensitive to small distance changes, thereby affecting how quickly the influences of the goal or obstacles diminish over distance .

The algorithm ensures obstacle avoidance by calculating forces of attraction and repulsion for the current position. The attraction to the goal is computed as a negative potential, pulling the path towards the goal, while the repulsion from obstacles provides a positive potential to keep the path clear of them. The variable 'JT' calculates the total influence of both attraction and repulsion to adjust the path effectively, avoiding obstacles while moving towards the goal .

The algorithm determines the best next point by calculating a fitness score for each potential point generated around the current position. This fitness score is based on the distances to both the goal and obstacles, considering both their attraction and repulsion potentials. The point with the highest fitness score is selected as the best point, updating the current position towards it .

The 'theta' variable signifies the minimum angle for distributing potential points around the current position. It is calculated as 360 divided by the number of points, ensuring an even distribution of these points in a circular manner around the current position, crucial for assessing all possible directions efficiently .

The while loop with the condition 'dGoal > 0.1' is used to iteratively adjust the path until the current position is sufficiently close to the goal. The threshold of 0.1 is chosen to ensure the loop continues to adjust the points generated towards the goal until it is almost reached, allowing for precise navigation while considering the presence of obstacles .

The radius of 0.1 is used to generate a set of potential points around the current position in the algorithm. This helps in evaluating these points to determine the best next step towards the goal while avoiding obstacles. This radius determines how far from the current position the potential points will be considered.

You might also like