penalty function for path planning of robot in multiple static obstacle environment

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