3-PSO算法（粒子群算法）的栅格路径寻优计算
3-PSO算法（粒子群算法）的栅格路径寻优计算链接：https://pan.baidu.com/s/1hEzx9dh3-En7_fWFWKfmTQ 密码：ryqw
具体链接在halcom.cn论坛，联系人QQ：3283892722
该论坛是一个学习交流平台，我会逐一的和大家分享学习。
欢迎大家录制视频，你可在论坛进行打赏分享。
视频专用播放器：http://halcom.cn/forum.php?mod=viewthread&tid=258&extra=page%3D1
主程序如下：clc,clear,close all
warning off
%% MAP
load('data2.mat');
K = 4; % 与节点自身相连的节点数
= init_map(data2,K);
%% 目标
startpoint=39;% 起始节点
endpoint=18; % 终止节点
%% PSO
c1 = 1.4995;
c2 = 1.4995;
Vmin = -1;
Vmax = 1;
maxiter = 20;% 迭代次数
sizepop = 20;% 种群数量
popmin1 = 1;popmax1 = 51; % x1
popmin2 = 1;popmax2 = 51; % x2
% 初始化种群
for i=1:sizepop
x1 = round( popmin1 + (popmax1-popmin1)*rand );
x2 = round( popmin2 + (popmax2-popmin2)*rand );
pop(i,1) = x1;
pop(i,2) = x2;
= fun_dijstra( , A, dij );
V(i,1) = 0;
V(i,2) = 0;
end
% 记录一组最优值
=min(fitness);
zbest.pop=pop(bestindex,:); % 全局最佳
gbest=pop; % 个体最佳
fitnessgbest=fitness; % 个体最佳适应度值
fitnesszbest=bestfitness; % 全局最佳适应度值
zbest.path = path{bestindex}; % 最佳个体对应的最优路径
wmax = 0.9;wmin = 0.4;
%% 迭代寻优
for i=1:maxiter
for j=1:sizepop
% 自适应权重1
% w = wmin + (wmax-wmin)*(fitnessgbest(j)-min(fitness))/( max(fitness)-min(fitness) );
% 自适应权重2
% w = wmin - (wmax-wmin)*(fitnessgbest(j)-min(fitness))/( mean(fitness)-min(fitness) );
% 自适应权重3
if fitnessgbest(j)<=mean(fitness)
w = wmin - (wmax-wmin)*(fitnessgbest(j)-min(fitness))/( mean(fitness)-min(fitness) );
else
w = wmax;
end
% 速度更新
V(j,:) = w*V(j,:) + c1*rand*(gbest(j,:) - pop(j,:)) + c2*rand*(zbest.pop - pop(j,:));
% V--x1
if V(j,1)>Vmax
V(j,1)=Vmax;
end
if V(j,1)<Vmin
V(j,1)=Vmin;
end
% V--x2
if V(j,2)>Vmax
V(j,2)=Vmax;
end
if V(j,2)<Vmin
V(j,2)=Vmin;
end
% 个体更新
% pop(j,:) = pop(j,:) + 0.5 * V(j,:);
pop(j,1) = round( pop(j,1) + 1.5 * V(j,1) );
pop(j,2) = round( pop(j,2) + 0.5 * V(j,2) );
% x1越界限制
if pop(j,1)>popmax1
pop(j,1)=popmax1;
end
if pop(j,1)<popmin1
pop(j,1)=popmin1;
end
% x2越界限制
if pop(j,2)>popmax2
pop(j,2)=popmax2;
end
if pop(j,2)<popmin2
pop(j,2)=popmin2;
end
% 适应度更新
= fun_dijstra( , A, dij );
% 比较个体间比较
if fitness(j)<fitnessgbest(j)
fitnessgbest(j) = fitness(j);
gbest(j,:) = pop(j,:);
end
if fitness(j)<bestfitness
bestfitness = fitness(j);
zbest.pop =pop(j,:);
zbest.path = path{j};
end
end
fitness_iter(i) = bestfitness;
end
toc ;
times = toc;
fprintf('\n')
disp(['计算时间 Time = ', num2str(times) ])
fprintf('\n')
disp(['最优解 ', num2str(zbest.pop)])
fprintf('\n')
disp(['最优解对应的最优路径 ', num2str(zbest.path)])
fprintf('\n')
figure('color',)
plot(fitness_iter,'ro-','linewidth',2)
%% 绘图
figure(3)
colormap(),pcolor(0.5:size(a,2)+0.5,0.5:size(a,1)+0.5,b)
hold on
% 节点网络结构初始化
for i=1:citynum
plot(x(i)+0.5,y(i)+0.5,'ro','MarkerEdgeColor','r','MarkerFaceColor','g','markersize',8);
hold on;
text(x(i)+0.5,y(i)+0.5+0.2,num2str(i),'Color',);
end
% 连线
for i=1:length(zbest.path)-1
plot(,,'b-','MarkerEdgeColor','r','MarkerFaceColor','g','markersize',8,'linewidth',2);
end
axis tight;
axis off;
hold off
适应度函数如下：
function = fun_dijstra( pop, A, dij )
path =[];
for i=1:length(pop)-1
% path1 = find_path2(pop(i), pop(i+1), A);% 找路径
path1 = dijkstra(pop(i), pop(i+1), dij);% 找路径
path = ;
end
% 删除重复的节点
index=[];
for i=1:length(path)-1
if(path(i)==path(i+1))
index=;
end
end
path(index)=[];
fitness = ca_tsp(path,dij);dijkstra最短路算法：
function = dijkstra(pathS, pathE, transmat)
% The Dijkstra's algorithm, Implemented by Yi Wang, 2005
% pathS: 所求最短路径的起点
% pathE :所求最短路径的终点
% transmat: 图的转移矩阵或者邻接矩阵，应为方阵
if ( size(transmat,1) ~= size(transmat,2) )
error( 'detect_cycles:Dijkstra_SC', ...
'transmat has different width and heights' );
end
% 初始化:
%noOfNode-图中的顶点数
%parent(i)-节点i的父节点
%distance(i)-从起点pathS的最短路径的长度
%queue-图的广度遍历
noOfNode = size(transmat, 1);
for i = 1:noOfNode
parent(i) = 0; % 初值操作
distance(i) = Inf; % 初值操作
end
queue = []; % 队列
% 由路径开始最短路计算
for i=1:noOfNode
if transmat(pathS, i)~=Inf
distance(i) = transmat(pathS, i);
parent(i) = pathS;% 当前路径
queue = ;
end
end
% 对图进行广度遍历
while length(queue) ~= 0
hopS= queue(1);
queue = queue(2:end);
for hopE = 1:noOfNode
if distance(hopE) > distance(hopS) + transmat(hopS,hopE) % 如果当前距离大于转换后的距离
distance(hopE) = distance(hopS) + transmat(hopS,hopE);% 更新
parent(hopE) = hopS;
queue = ;
end
end
end
% 回溯进行最短路径的查找
r_path = ;
i = parent(pathE);
while i~=pathS && i~=0
r_path = ;
i = parent(i);
end
if i==pathS
r_path = ; % 记录
else
r_path = [] % 清空
end
% 返回最短路径的权和
r_cost = distance(pathE); 距离计算：
% ca_tsp.m
% 计算路径长度
function ltsp=ca_tsp(c,dij)
i=1;
ltsp=0;
while i<length(c);
ltsp=ltsp+dij(c(i),c(i+1));
i=i+1;
end
end
参考：
【1】基于穷举法的机器人避障路径寻优（免费）
【2】智能车辆局部避障路径规划及横向运动控制研究_陈东
PSO算法（粒子群算法）的栅格路径寻优计算 楼主好人。。。。 值得学习。。。。。。 认真学习代码！ 认真学习代码，努力毕业，加油。 认真学习一下 PSO算法（粒子群算法） 谢谢楼主。。。。。。 顶一下,新手刚开始学。。。。。。。