3-PSO算法（粒子群算法）的栅格路径寻优计算

Halcom

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

主程序如下：

1. clc,clear,close all
   
2. warning off
   
3. %% MAP
   
4. load('data2.mat');
   
5. K = 4;             % 与节点自身相连的节点数
   
6. [x,y,A,dij, citynum,a,b] = init_map(data2,K);
   
7. %% 目标
   
8. startpoint=39;  % 起始节点
   
9. endpoint=18;    % 终止节点
   
10. %% PSO
    
11. c1 = 1.4995;  
    
12. c2 = 1.4995;
    
13. Vmin = -1;
    
14. Vmax = 1;
    
15. maxiter = 20;  % 迭代次数
    
16. sizepop = 20;  % 种群数量
    
17. popmin1 = 1;  popmax1 = 51; % x1
    
18. popmin2 = 1;  popmax2 = 51; % x2
    
19. % 初始化种群
    
20. for i=1:sizepop
    
21.     x1 = round( popmin1 + (popmax1-popmin1)*rand );
    
22.     x2 = round( popmin2 + (popmax2-popmin2)*rand );
    
23.     pop(i,1) = x1;
    
24.     pop(i,2) = x2;
    
25.     [path{i}, fitness(i)] = fun_dijstra( [startpoint, pop(i,:), endpoint], A, dij );
    
26.     V(i,1) = 0;
    
27.     V(i,2) = 0;
    
28. end
    
29. % 记录一组最优值
    
30. [bestfitness,bestindex]=min(fitness);
    
31. zbest.pop=pop(bestindex,:);   % 全局最佳
    
32. gbest=pop;                % 个体最佳
    
33. fitnessgbest=fitness;     % 个体最佳适应度值
    
34. fitnesszbest=bestfitness; % 全局最佳适应度值
    
35. zbest.path = path{bestindex};   % 最佳个体对应的最优路径
    
36. wmax = 0.9;  wmin = 0.4;
    
37. %% 迭代寻优
    
38. for i=1:maxiter
    
39.     for j=1:sizepop
    
40.         % 自适应权重1
    
41. %         w = wmin + (wmax-wmin)*(fitnessgbest(j)-min(fitness))/( max(fitness)-min(fitness) );
    
42.         % 自适应权重2
    
43. %         w = wmin - (wmax-wmin)*(fitnessgbest(j)-min(fitness))/( mean(fitness)-min(fitness) );
    
44.         % 自适应权重3
    
45.         if fitnessgbest(j)<=mean(fitness)
    
46.             w = wmin - (wmax-wmin)*(fitnessgbest(j)-min(fitness))/( mean(fitness)-min(fitness) );
    
47.         else
    
48.             w = wmax;
    
49.         end
    
50.         % 速度更新
    
51.         V(j,:) = w*V(j,:) + c1*rand*(gbest(j,:) - pop(j,:)) + c2*rand*(zbest.pop - pop(j,:));
    
52.         % V--x1
    
53.         if V(j,1)>Vmax
    
54.             V(j,1)=Vmax;
    
55.         end
    
56.         if V(j,1)<Vmin
    
57.             V(j,1)=Vmin;
    
58.         end
    
59.         % V--x2
    
60.         if V(j,2)>Vmax
    
61.             V(j,2)=Vmax;
    
62.         end
    
63.         if V(j,2)<Vmin
    
64.             V(j,2)=Vmin;
    
65.         end
    
66.         % 个体更新
    
67. %         pop(j,:) = pop(j,:) + 0.5 * V(j,:);
    
68.         pop(j,1) = round( pop(j,1) + 1.5 * V(j,1) );
    
69.         pop(j,2) = round( pop(j,2) + 0.5 * V(j,2) );
    
70.         
    
71.         % x1  越界限制
    
72.         if pop(j,1)>popmax1
    
73.             pop(j,1)=popmax1;
    
74.         end
    
75.         if pop(j,1)<popmin1
    
76.             pop(j,1)=popmin1;
    
77.         end
    
78.         % x2  越界限制
    
79.         if pop(j,2)>popmax2
    
80.             pop(j,2)=popmax2;
    
81.         end
    
82.         if pop(j,2)<popmin2
    
83.             pop(j,2)=popmin2;
    
84.         end
    
85.         
    
86.         % 适应度更新
    
87.         [path{j}, fitness(j)] = fun_dijstra( [startpoint, pop(j,:), endpoint], A, dij );
    
88.         
    
89.         % 比较  个体间比较
    
90.         if fitness(j)<fitnessgbest(j)
    
91.             fitnessgbest(j) = fitness(j);
    
92.             gbest(j,:) = pop(j,:);
    
93.         end
    
94.         if fitness(j)<bestfitness
    
95.             bestfitness = fitness(j);
    
96.             zbest.pop =  pop(j,:);
    
97.             zbest.path = path{j};
    
98.         end
    
99.         
    
100.     end
     
101.     fitness_iter(i) = bestfitness;
     
102.    
     
103. end
     
104. toc ;
     
105. times = toc;
     
106. fprintf('\n')
     
107. disp(['计算时间 Time =    ',   num2str(times) ])
     
108. fprintf('\n')
     
109. disp(['最优解   ', num2str(zbest.pop)])
     
110. fprintf('\n')
     
111. disp(['最优解对应的最优路径   ', num2str(zbest.path)])
     
112. fprintf('\n')
     
113. 
     
114. figure('color',[1,1,1])
     
115. plot(fitness_iter,'ro-','linewidth',2)
     
116. 
     
117. %% 绘图
     
118. figure(3)
     
119. colormap([0 0 0;1 1 1]),pcolor(0.5:size(a,2)+0.5,0.5:size(a,1)+0.5,b)
     
120. hold on
     
121. % 节点网络结构初始化
     
122. for i=1:citynum
     
123.     plot(x(i)+0.5,y(i)+0.5,'ro','MarkerEdgeColor','r','MarkerFaceColor','g','markersize',8);
     
124.     hold on;
     
125.     text(x(i)+0.5,y(i)+0.5+0.2,num2str(i),'Color',[1 0 0]);
     
126. end
     
127. % 连线
     
128. for i=1:length(zbest.path)-1
     
129.     plot([x(zbest.path(i))+0.5,x(zbest.path(i+1))+0.5],[y(zbest.path(i))+0.5,y(zbest.path(i+1))+0.5],'b-','MarkerEdgeColor','r','MarkerFaceColor','g','markersize',8,'linewidth',2);
     
130. end
     
131. axis tight;
     
132. axis off;
     
133. hold off
     

复制代码

适应度函数如下：

1. function [path, fitness] = fun_dijstra( pop, A, dij )
   
2. path =[];
   
3. for i=1:length(pop)-1
   
4. %     path1 = find_path2(pop(i), pop(i+1), A);  % 找路径
   
5.     path1 = dijkstra(pop(i), pop(i+1), dij);  % 找路径
   
6.     path = [path,path1];
   
7. end
   
8. 
   
9. % 删除重复的节点
   
10. index=[];
    
11. for i=1:length(path)-1
    
12.     if(path(i)==path(i+1))
    
13.         index=[index,i];
    
14.     end
    
15. end
    
16. path(index)=[];
    
17. 
    
18. fitness = ca_tsp(path,dij);

复制代码

dijkstra最短路算法：

1. function [r_path, r_cost] = dijkstra(pathS, pathE, transmat)
   
2. % The Dijkstra's algorithm, Implemented by Yi Wang, 2005
   
3. %   pathS: 所求最短路径的起点
   
4. %   pathE :所求最短路径的终点
   
5. %   transmat: 图的转移矩阵或者邻接矩阵，应为方阵
   
6. if ( size(transmat,1) ~= size(transmat,2) )
   
7.   error( 'detect_cycles:Dijkstra_SC', ...
   
8.          'transmat has different width and heights' );
   
9. end
   
10. % 初始化:
    
11. %  noOfNode-图中的顶点数
    
12. %  parent(i)-节点i的父节点
    
13. %  distance(i)-从起点pathS的最短路径的长度
    
14. %  queue-图的广度遍历
    
15. noOfNode = size(transmat, 1);
    
16. for i = 1:noOfNode
    
17.   parent(i) = 0;       % 初值操作
    
18.   distance(i) = Inf;    % 初值操作
    
19. end
    
20. queue = [];           % 队列
    
21. 
    
22. % 由路径开始最短路计算
    
23. for i=1:noOfNode
    
24.   if transmat(pathS, i)~=Inf
    
25.     distance(i) = transmat(pathS, i);
    
26.     parent(i)   = pathS;  % 当前路径
    
27.     queue     = [queue i];
    
28.   end
    
29. end
    
30. 
    
31. % 对图进行广度遍历
    
32. while length(queue) ~= 0
    
33.   hopS  = queue(1);
    
34.   queue = queue(2:end);
    
35.   
    
36.   for hopE = 1:noOfNode
    
37.     if distance(hopE) > distance(hopS) + transmat(hopS,hopE)   % 如果当前距离大于转换后的距离
    
38.       distance(hopE) = distance(hopS) + transmat(hopS,hopE);  % 更新
    
39.       parent(hopE)   = hopS;
    
40.       queue         = [queue hopE];
    
41.     end
    
42.   end
    
43.   
    
44. end
    
45. 
    
46. % 回溯进行最短路径的查找
    
47. r_path = [pathE];   
    
48. i = parent(pathE);
    
49. 
    
50. while i~=pathS && i~=0
    
51.   r_path = [i r_path];
    
52.   i      = parent(i);
    
53. end
    
54. if i==pathS
    
55.   r_path = [i r_path];   % 记录
    
56. else
    
57.   r_path = []          % 清空
    
58. end
    
59. % 返回最短路径的权和
    
60. r_cost = distance(pathE);

复制代码

距离计算：

1. % ca_tsp.m
   
2. % 计算路径长度
   
3. function ltsp=ca_tsp(c,dij)
   
4.     i=1;
   
5.     ltsp=0;
   
6.     while i<length(c);
   
7.         ltsp=ltsp+dij(c(i),c(i+1));
   
8.         i=i+1;
   
9.     end
   
10. end
    

复制代码

参考：
【1】基于穷举法的机器人避障路径寻优（免费）
【2】智能车辆局部避障路径规划及横向运动控制研究_陈东

1370167440

PSO算法（粒子群算法）的栅格路径寻优计算

win2018

楼主好人。。。。

win2018

值得学习。。。。。。

captain

认真学习代码！

captain

认真学习代码，努力毕业，加油。

zhougc

认真学习一下

吴江进

PSO算法（粒子群算法）

wdd123456

谢谢楼主。。。。。。

wdd123456

顶一下,新手刚开始学。。。。。。。