penalty function for path planning of robot in multiple static obstacle environment
1 次查看(过去 30 天)
显示 更早的评论
hi
can any one knows how to add penalty function in the target for path planning of robot using Particle swarm optimization. what i have do until now is following:
clear all
clc
hold on
d=5
pos = [70-d/2 70-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [70-d/2 90-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [10-d/2 10-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [30-d/2 40-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [40-d/2 50-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [70-d/2 40-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [15-d/2 15-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [25-d/2 25-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [10-d/2 85-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [95-d/2 75-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [50-d/2 50-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [80-d/2 75-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [80-d/2 90-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [80-d/2 25-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [50-d/2 10-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [40-d/2 10-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [40-d/2 20-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [20-d/2 40-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
pos = [30-d/2 60-d/2 d d];
rectangle('Position',pos,'Curvature',[1 1])
obstaclex=[70 70 10 30 40 70 15 25 10 95 50 80 80 80 50 40 40 20 30]
obstacley=[70 90 10 40 50 40 15 25 85 75 50 75 90 25 10 10 20 40 60]
obstacle=19
axis([0 100 0 100])
robot1=[0,0];
temp=[0,0];
p1=[1,1;2,2;3,3;1,2;1,3;2,1;2,2;2,3;3,1;3,2]
target=[95,95]
totalparticles=10
% barrier
for h=1:totalparticles
for n=1:obstacle
g(h,n)=1/(-((obstaclex(1,n)-p1(h,1))^2)-((obstacley(1,n)-p1(h,2))^2)+25)
end
end
%penalty
for h=1:totalparticles
for n=1:obstacle
x(h,n)=sqrt((obstaclex(1,n)-p1(h,1))^2+(obstacley(1,n)-p1(h,2))^2)
end
end
for h=1:totalparticles
for n=1:obstacle
rb(h,n)=.5-exp(-2*x(h,n))
end
end
penalty=x.*rb
now, how i process it further. algorithm for barrier and penalty is attached herewith
0 个评论
回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息
产品
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!