├── .gitattributes ├── .gitignore ├── CreateModel.m ├── CreateRandomSolution.m ├── MyCost.m ├── PSO AND ROBOT PATH PLANNING PROBLEM.docx ├── ParseSolution.m ├── PlotSolution.m └── pso.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear in the root of a volume 35 | .DocumentRevisions-V100 36 | .fseventsd 37 | .Spotlight-V100 38 | .TemporaryItems 39 | .Trashes 40 | .VolumeIcon.icns 41 | 42 | # Directories potentially created on remote AFP share 43 | .AppleDB 44 | .AppleDesktop 45 | Network Trash Folder 46 | Temporary Items 47 | .apdisk 48 | -------------------------------------------------------------------------------- /CreateModel.m: -------------------------------------------------------------------------------- 1 | 2 | function model=CreateModel() 3 | 4 | % Source 5 | xs=0; 6 | ys=0; 7 | 8 | % Target (Destination) 9 | xt=4; 10 | yt=6; 11 | 12 | xobs=[1.5 4.0 1.2]; 13 | yobs=[4.5 3.0 1.5]; 14 | robs=[1.5 1.0 0.8]; 15 | 16 | n=3; 17 | 18 | xmin=-10; 19 | xmax= 10; 20 | 21 | ymin=-10; 22 | ymax= 10; 23 | 24 | model.xs=xs; 25 | model.ys=ys; 26 | model.xt=xt; 27 | model.yt=yt; 28 | model.xobs=xobs; 29 | model.yobs=yobs; 30 | model.robs=robs; 31 | model.n=n; 32 | model.xmin=xmin; 33 | model.xmax=xmax; 34 | model.ymin=ymin; 35 | model.ymax=ymax; 36 | 37 | end -------------------------------------------------------------------------------- /CreateRandomSolution.m: -------------------------------------------------------------------------------- 1 | 2 | function sol1=CreateRandomSolution(model) 3 | 4 | n=model.n; 5 | 6 | xmin=model.xmin; 7 | xmax=model.xmax; 8 | 9 | ymin=model.ymin; 10 | ymax=model.ymax; 11 | 12 | sol1.x=unifrnd(xmin,xmax,1,n); 13 | sol1.y=unifrnd(ymin,ymax,1,n); 14 | 15 | end -------------------------------------------------------------------------------- /MyCost.m: -------------------------------------------------------------------------------- 1 | 2 | function [z, sol]=MyCost(sol1,model) 3 | 4 | sol=ParseSolution(sol1,model); 5 | 6 | beta=100; 7 | z=sol.L*(1+beta*sol.Violation); 8 | 9 | end -------------------------------------------------------------------------------- /PSO AND ROBOT PATH PLANNING PROBLEM.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumanismcse/Path-Planning/b8189f58b9b3fcb1ee44a85224e1c45e5a4e5c90/PSO AND ROBOT PATH PLANNING PROBLEM.docx -------------------------------------------------------------------------------- /ParseSolution.m: -------------------------------------------------------------------------------- 1 | 2 | function sol2=ParseSolution(sol1,model) 3 | 4 | x=sol1.x; 5 | y=sol1.y; 6 | 7 | xs=model.xs; 8 | ys=model.ys; 9 | xt=model.xt; 10 | yt=model.yt; 11 | xobs=model.xobs; 12 | yobs=model.yobs; 13 | robs=model.robs; 14 | 15 | XS=[xs x xt]; 16 | YS=[ys y yt]; 17 | k=numel(XS); 18 | TS=linspace(0,1,k); 19 | 20 | tt=linspace(0,1,100); 21 | xx=spline(TS,XS,tt); 22 | yy=spline(TS,YS,tt); 23 | 24 | dx=diff(xx); 25 | dy=diff(yy); 26 | 27 | L=sum(sqrt(dx.^2+dy.^2)); 28 | 29 | nobs = numel(xobs); % Number of Obstacles 30 | Violation = 0; 31 | for k=1:nobs 32 | d=sqrt((xx-xobs(k)).^2+(yy-yobs(k)).^2); 33 | v=max(1-d/robs(k),0); 34 | Violation=Violation+mean(v); 35 | end 36 | 37 | sol2.TS=TS; 38 | sol2.XS=XS; 39 | sol2.YS=YS; 40 | sol2.tt=tt; 41 | sol2.xx=xx; 42 | sol2.yy=yy; 43 | sol2.dx=dx; 44 | sol2.dy=dy; 45 | sol2.L=L; 46 | sol2.Violation=Violation; 47 | sol2.IsFeasible=(Violation==0); 48 | 49 | 50 | end 51 | -------------------------------------------------------------------------------- /PlotSolution.m: -------------------------------------------------------------------------------- 1 | 2 | function PlotSolution(sol,model) 3 | 4 | xs=model.xs; 5 | ys=model.ys; 6 | xt=model.xt; 7 | yt=model.yt; 8 | xobs=model.xobs; 9 | yobs=model.yobs; 10 | robs=model.robs; 11 | 12 | XS=sol.XS; 13 | YS=sol.YS; 14 | xx=sol.xx; 15 | yy=sol.yy; 16 | 17 | theta=linspace(0,2*pi,100); 18 | for k=1:numel(xobs) 19 | fill(xobs(k)+robs(k)*cos(theta),yobs(k)+robs(k)*sin(theta),[0.5 0.7 0.8]); 20 | hold on; 21 | end 22 | plot(xx,yy,'k','LineWidth',2); 23 | plot(XS,YS,'ro'); 24 | plot(xs,ys,'bs','MarkerSize',12,'MarkerFaceColor','y'); 25 | plot(xt,yt,'kp','MarkerSize',16,'MarkerFaceColor','g'); 26 | hold off; 27 | grid on; 28 | axis equal; 29 | 30 | end -------------------------------------------------------------------------------- /pso.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; 3 | close all; 4 | 5 | %% Problem Definition 6 | 7 | model=CreateModel(); 8 | 9 | model.n=3; % number of Handle Points 10 | 11 | CostFunction=@(x) MyCost(x,model); % Cost Function 12 | 13 | nVar=model.n; % Number of Decision Variables 14 | 15 | VarSize=[1 nVar]; % Size of Decision Variables Matrix 16 | 17 | VarMin.x=model.xmin; % Lower limit of Variables 18 | VarMax.x=model.xmax; % Upper limit of Variables 19 | VarMin.y=model.ymin; % Lower limit of Variables 20 | VarMax.y=model.ymax; % Upper limit of Variables 21 | 22 | 23 | %% PSO Parameters 24 | 25 | MaxIt=50; % Maximum Number of Iterations 26 | 27 | nPop=15; % Population Size (Swarm Size) 28 | 29 | w=1; % Inertia Weight 30 | wdamp=0.98; % Inertia Weight Damping Ratio 31 | c1=1.5; % Personal Learning Coefficient 32 | c2=1.5; % Global Learning Coefficient 33 | 34 | alpha=0.1; 35 | VelMax.x=alpha*(VarMax.x-VarMin.x); % Maximum Velocity 36 | VelMin.x=-VelMax.x; % Minimum Velocity 37 | VelMax.y=alpha*(VarMax.y-VarMin.y); % Maximum Velocity 38 | VelMin.y=-VelMax.y; % Minimum Velocity 39 | 40 | %% Initialization 41 | 42 | % Create Empty Particle Structure 43 | empty_particle.Position=[]; 44 | empty_particle.Velocity=[]; 45 | empty_particle.Cost=[]; 46 | empty_particle.Sol=[]; 47 | empty_particle.Best.Position=[]; 48 | empty_particle.Best.Cost=[]; 49 | empty_particle.Best.Sol=[]; 50 | 51 | % Initialize Global Best 52 | GlobalBest.Cost=inf; 53 | 54 | % Create Particles Matrix 55 | particle=repmat(empty_particle,nPop,1); 56 | 57 | % Initialization Loop 58 | for i=1:nPop 59 | 60 | % Initialize Position 61 | if i > 1 62 | particle(i).Position=CreateRandomSolution(model); 63 | else 64 | % Straight line from source to destination 65 | xx = linspace(model.xs, model.xt, model.n+2); 66 | yy = linspace(model.ys, model.yt, model.n+2); 67 | particle(i).Position.x = xx(2:end-1); 68 | particle(i).Position.y = yy(2:end-1); 69 | end 70 | 71 | % Initialize Velocity 72 | particle(i).Velocity.x=zeros(VarSize); 73 | particle(i).Velocity.y=zeros(VarSize); 74 | 75 | % Evaluation 76 | [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position); 77 | 78 | % Update Personal Best 79 | particle(i).Best.Position=particle(i).Position; 80 | particle(i).Best.Cost=particle(i).Cost; 81 | particle(i).Best.Sol=particle(i).Sol; 82 | 83 | % Update Global Best 84 | if particle(i).Best.CostVarMax.x); 117 | particle(i).Velocity.x(OutOfTheRange)=-particle(i).Velocity.x(OutOfTheRange); 118 | 119 | % Update Position Bounds 120 | particle(i).Position.x = max(particle(i).Position.x,VarMin.x); 121 | particle(i).Position.x = min(particle(i).Position.x,VarMax.x); 122 | 123 | % y Part 124 | 125 | % Update Velocity 126 | particle(i).Velocity.y = w*particle(i).Velocity.y ... 127 | + c1*rand(VarSize).*(particle(i).Best.Position.y-particle(i).Position.y) ... 128 | + c2*rand(VarSize).*(GlobalBest.Position.y-particle(i).Position.y); 129 | 130 | % Update Velocity Bounds 131 | particle(i).Velocity.y = max(particle(i).Velocity.y,VelMin.y); 132 | particle(i).Velocity.y = min(particle(i).Velocity.y,VelMax.y); 133 | 134 | % Update Position 135 | particle(i).Position.y = particle(i).Position.y + particle(i).Velocity.y; 136 | 137 | % Velocity Mirroring 138 | OutOfTheRange=(particle(i).Position.yVarMax.y); 139 | particle(i).Velocity.y(OutOfTheRange)=-particle(i).Velocity.y(OutOfTheRange); 140 | 141 | % Update Position Bounds 142 | particle(i).Position.y = max(particle(i).Position.y,VarMin.y); 143 | particle(i).Position.y = min(particle(i).Position.y,VarMax.y); 144 | 145 | % Evaluation 146 | [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position); 147 | 148 | % Update Personal Best 149 | if particle(i).Cost