愿你搜索到人生的最优解
hello，大家好。各位可点击左下方阅读原文，访问公众号官方店铺。谨防上当受骗，感谢各位支持！
我们在MATLAB数学建模（九） | 粒子群优化（PSO）算法求解二维路径规划问题（附MATLAB代码）这篇推文中讲解了PSO求解二维路径规划问题，并给出了相应的MATLAB代码，但不少小伙伴反应代码有些难理解，所以今天我们详细讲解一下这份MATLAB代码。
讲解视频地址：
MATLAB代码源地址：
https://yarpiz.com/403/ypap115-path-planning
本期推文目录：
01 主函数（pso函数）
02 CreateModel函数
03 MyCost函数
04 ParseSolution函数
05 CreateRandomSolution函数
06 PlotSolution函数
01 | 主函数（pso函数）
主函数（pso函数）
%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: [email protected], [email protected]
%
clc;
clear;
close all;
%% Problem Definition
model=CreateModel();
model.n=3; % number of Handle Points
CostFunction=@(x) MyCost(x,model); % Cost Function
nVar=model.n; % Number of Decision Variables
VarSize=[1 nVar]; % Size of Decision Variables Matrix
VarMin.x=model.xmin; % Lower Bound of Variables
VarMax.x=model.xmax; % Upper Bound of Variables
VarMin.y=model.ymin; % Lower Bound of Variables
VarMax.y=model.ymax; % Upper Bound of Variables
%% PSO Parameters
MaxIt=500; % Maximum Number of Iterations
nPop=150; % Population Size (Swarm Size)
w=1; % Inertia Weight
wdamp=0.98; % Inertia Weight Damping Ratio
c1=1.5; % Personal Learning Coefficient
c2=1.5; % Global Learning Coefficient
% % Constriction Coefficient
% phi1=2.05;
% phi2=2.05;
% phi=phi1+phi2;
% chi=2/(phi-2+sqrt(phi^2-4*phi));
% w=chi; % Inertia Weight
% wdamp=1; % Inertia Weight Damping Ratio
% c1=chi*phi1; % Personal Learning Coefficient
% c2=chi*phi2; % Global Learning Coefficient
alpha=0.1;
VelMax.x=alpha*(VarMax.x-VarMin.x); % Maximum Velocity
VelMin.x=-VelMax.x; % Minimum Velocity
VelMax.y=alpha*(VarMax.y-VarMin.y); % Maximum Velocity
VelMin.y=-VelMax.y; % Minimum Velocity
%% Initialization
% Create Empty Particle Structure
empty_particle.Position=[];
empty_particle.Velocity=[];
empty_particle.Cost=[];
empty_particle.Sol=[];
empty_particle.Best.Position=[];
empty_particle.Best.Cost=[];
empty_particle.Best.Sol=[];
% Initialize Global Best
GlobalBest.Cost=inf;
% Create Particles Matrix
particle=repmat(empty_particle,nPop,1);
% Initialization Loop
for i=1:nPop
% Initialize Position
if i > 1
particle(i).Position=CreateRandomSolution(model);
else
% Straight line from source to destination
xx = linspace(model.xs, model.xt, model.n+2);
yy = linspace(model.ys, model.yt, model.n+2);
particle(i).Position.x = xx(2:end-1);
particle(i).Position.y = yy(2:end-1);
end
% Initialize Velocity
particle(i).Velocity.x=zeros(VarSize);
particle(i).Velocity.y=zeros(VarSize);
% Evaluation
[particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
% Update Personal Best
particle(i).Best.Position=particle(i).Position;
particle(i).Best.Cost=particle(i).Cost;
particle(i).Best.Sol=particle(i).Sol;
% Update Global Best
if particle(i).Best.Cost<GlobalBest.Cost
GlobalBest=particle(i).Best;
end
end
% Array to Hold Best Cost Values at Each Iteration
BestCost=zeros(MaxIt,1);
%% PSO Main Loop
for it=1:MaxIt
for i=1:nPop
% x Part
% Update Velocity
particle(i).Velocity.x = w*particle(i).Velocity.x ...
+ c1*rand(VarSize).*(particle(i).Best.Position.x-particle(i).Position.x) ...
+ c2*rand(VarSize).*(GlobalBest.Position.x-particle(i).Position.x);
% Update Velocity Bounds
particle(i).Velocity.x = max(particle(i).Velocity.x,VelMin.x);
particle(i).Velocity.x = min(particle(i).Velocity.x,VelMax.x);
% Update Position
particle(i).Position.x = particle(i).Position.x + particle(i).Velocity.x;
% Velocity Mirroring
OutOfTheRange=(particle(i).Position.x<VarMin.x | particle(i).Position.x>VarMax.x);
particle(i).Velocity.x(OutOfTheRange)=-particle(i).Velocity.x(OutOfTheRange);
% Update Position Bounds
particle(i).Position.x = max(particle(i).Position.x,VarMin.x);
particle(i).Position.x = min(particle(i).Position.x,VarMax.x);
% y Part
% Update Velocity
particle(i).Velocity.y = w*particle(i).Velocity.y ...
+ c1*rand(VarSize).*(particle(i).Best.Position.y-particle(i).Position.y) ...
+ c2*rand(VarSize).*(GlobalBest.Position.y-particle(i).Position.y);
% Update Velocity Bounds
particle(i).Velocity.y = max(particle(i).Velocity.y,VelMin.y);
particle(i).Velocity.y = min(particle(i).Velocity.y,VelMax.y);
% Update Position
particle(i).Position.y = particle(i).Position.y + particle(i).Velocity.y;
% Velocity Mirroring
OutOfTheRange=(particle(i).Position.y<VarMin.y | particle(i).Position.y>VarMax.y);
particle(i).Velocity.y(OutOfTheRange)=-particle(i).Velocity.y(OutOfTheRange);
% Update Position Bounds
particle(i).Position.y = max(particle(i).Position.y,VarMin.y);
particle(i).Position.y = min(particle(i).Position.y,VarMax.y);
% Evaluation
[particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
% Update Personal Best
if particle(i).Cost<particle(i).Best.Cost
particle(i).Best.Position=particle(i).Position;
particle(i).Best.Cost=particle(i).Cost;
particle(i).Best.Sol=particle(i).Sol;
% Update Global Best
if particle(i).Best.Cost<GlobalBest.Cost
GlobalBest=particle(i).Best;
end
end
end
% Update Best Cost Ever Found
BestCost(it)=GlobalBest.Cost;
% Inertia Weight Damping
w=w*wdamp;
% Show Iteration Information
if GlobalBest.Sol.IsFeasible
Flag=' *';
else
Flag=[', Violation = ' num2str(GlobalBest.Sol.Violation)];
end
disp(['Iteration ' num2str(it) ': Best Cost = ' num2str(BestCost(it)) Flag]);
% Plot Solution
figure(1);
PlotSolution(GlobalBest.Sol,model);
pause(0.01);
end
%% Results
figure;
plot(BestCost,'LineWidth',2);
xlabel('Iteration');
ylabel('Best Cost');
grid on;
02 | CreateModel函数
CreateModel函数的作用是创建路径规划的环境，包括确定起点和终点坐标、3个障碍物圆心的坐标、3个障碍物的半径。
%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: [email protected], [email protected]
%
function model=CreateModel()
% Source
xs=0;
ys=0;
% Target (Destination)
xt=4;
yt=6;
xobs=[1.5 4.0 1.2];
yobs=[4.5 3.0 1.5];
robs=[1.5 1.0 0.8];
n=3;
xmin=-10;
xmax= 10;
ymin=-10;
ymax= 10;
model.xs=xs;
model.ys=ys;
model.xt=xt;
model.yt=yt;
model.xobs=xobs;
model.yobs=yobs;
model.robs=robs;
model.n=n;
model.xmin=xmin;
model.xmax=xmax;
model.ymin=ymin;
model.ymax=ymax;
end
03 | MyCost函数
MyCost函数的作用是计算一条路线的成本，包含这条路线的距离和违反约束的成本。
%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: [email protected], [email protected]
%
function [z, sol]=MyCost(sol1,model)
sol=ParseSolution(sol1,model);
beta=1e7;
z=sol.L*(1+beta*sol.Violation);
end
04 | ParseSolution函数
ParseSolution函数的作用是将少量离散的行走点扩充到大量离散行走点，具体涉及到linspace函数和spline函数。
%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: [email protected], [email protected]
%
function sol2=ParseSolution(sol1,model)
x=sol1.x;
y=sol1.y;
xs=model.xs;
ys=model.ys;
xt=model.xt;
yt=model.yt;
xobs=model.xobs;
yobs=model.yobs;
robs=model.robs;
XS=[xs x xt];
YS=[ys y yt];
k=numel(XS);
TS=linspace(0,1,k);
tt=linspace(0,1,5000);
xx=spline(TS,XS,tt);
yy=spline(TS,YS,tt);
dx=diff(xx);
dy=diff(yy);
L=sum(sqrt(dx.^2+dy.^2));
nobs = numel(xobs); % Number of Obstacles
Violation = 0;
for k=1:nobs
d=sqrt((xx-xobs(k)).^2+(yy-yobs(k)).^2);
v=max(1-d/robs(k),0);
Violation=Violation+mean(v);
end
sol2.TS=TS;
sol2.XS=XS;
sol2.YS=YS;
sol2.tt=tt;
sol2.xx=xx;
sol2.yy=yy;
sol2.dx=dx;
sol2.dy=dy;
sol2.L=L;
sol2.Violation=Violation;
sol2.IsFeasible=(Violation==0);
% figure;
% plot(xx,yy);
% hold on;
% plot(XS,YS,'ro');
% xlabel('x');
% ylabel('y');
%
% figure;
% plot(tt,xx);
% hold on;
% plot(TS,XS,'ro');
% xlabel('t');
% ylabel('x');
%
% figure;
% plot(tt,yy);
% hold on;
% plot(TS,YS,'ro');
% xlabel('t');
% ylabel('y');
end
05 | CreateRandomSolution函数
CreateRandomSolution函数的作用是创建随机解，具体涉及到unifrnd函数。
%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: [email protected], [email protected]
%
function sol1=CreateRandomSolution(model)
n=model.n;
xmin=model.xmin;
xmax=model.xmax;
ymin=model.ymin;
ymax=model.ymax;
sol1.x=unifrnd(xmin,xmax,1,n);
sol1.y=unifrnd(ymin,ymax,1,n);
end
06 | PlotSolution函数
画图函数：
%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB
% Publisher: Yarpiz (www.yarpiz.com)
%
% Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team)
%
% Contact Info: [email protected], [email protected]
%
function PlotSolution(sol,model)
xs=model.xs;
ys=model.ys;
xt=model.xt;
yt=model.yt;
xobs=model.xobs;
yobs=model.yobs;
robs=model.robs;
XS=sol.XS;
YS=sol.YS;
xx=sol.xx;
yy=sol.yy;
theta=linspace(0,2*pi,100);
for k=1:numel(xobs)
fill(xobs(k)+robs(k)*cos(theta),yobs(k)+robs(k)*sin(theta),[0.5 0.7 0.8]);
hold on;
end
plot(xx,yy,'k','LineWidth',2);
plot(XS,YS,'ro');
plot(xs,ys,'bs','MarkerSize',12,'MarkerFaceColor','y');
plot(xt,yt,'kp','MarkerSize',16,'MarkerFaceColor','g');
hold off;
grid on;
axis equal;
end
各位小伙伴可在留言板留言，未来我们讲解的具体内容由你做主。如果可以的话，可以把希望讲解的文献也在留言板上写出来。
