[数据结构与算法] 【路径规划】基于粒子群算法实现机器人栅格地图路径规划matlab源码

开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 数据结构与算法 -> 【路径规划】基于粒子群算法实现机器人栅格地图路径规划matlab源码 -> 正文阅读

[数据结构与算法]【路径规划】基于粒子群算法实现机器人栅格地图路径规划matlab源码

1 粒子群算法

模型参考这里。

3 代码

%% 粒子群
clc;
close all
clear
load('data4.mat')
figure(1)%画障碍图
hold on
S=(S_coo(2)-0.5)*num_shange+(S_coo(1)+0.5);%起点对应的编号
E=(E_coo(2)-0.5)*num_shange+(E_coo(1)+0.5);%终点对应的编号
for i=1:num_shange
 ? ?for j=1:num_shange
 ? ? ? ?if sign(i,j)==1
 ? ? ? ? ? ?y=[i-1,i-1,i,i];
 ? ? ? ? ? ?x=[j-1,j,j,j-1];
 ? ? ? ? ? ?h=fill(x,y,'k');
 ? ? ? ? ? ?set(h,'facealpha',0.5)
 ? ? ? ?end
 ? ? ? ?s=(num2str((i-1)*num_shange+j));
 ? ? ? ?%text(j-0.95,i-0.5,s,'fontsize',6)
 ? ?end
end
axis([0 num_shange 0 num_shange])%限制图的边界
plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点
plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点
set(gca,'YDir','reverse');%图像翻转
for i=1:num_shange
 ? ?plot([0 num_shange],[i-1 i-1],'k-');
 ? ?plot([i i],[0 num_shange],'k-');%画网格线
end
PopSize=20;%种群大小
OldBestFitness=0;%旧的最优适应度值
gen=0;%迭代次数
maxgen =100;%最大迭代次数
k1 = 1;%交叉1
k3 = 1;%交叉2
% c1=0.5;%交叉概率
Pm=0.7;%变异概率
c1=0.5;%认知系数
c2=0.7;%社会学习系数
w=0.96;%惯性系数
w_min=0.5;
w_max=1;
%%
%初始化路径
?
?
%最优解
index1=find(best_route==E);
route_lin=best_route(1:index1);
for i=2:index1
 ? ?Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];
 ? ?Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];
 ? ?plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3)
end
title('粒子群-遗传算法-对比路线');
figure(3)
hold on
for i=1:num_shange
 ? ?for j=1:num_shange
 ? ? ? ?if sign(i,j)==1
 ? ? ? ? ? ?y=[i-1,i-1,i,i];
 ? ? ? ? ? ?x=[j-1,j,j,j-1];
 ? ? ? ? ? ?h=fill(x,y,'k');
 ? ? ? ? ? ?set(h,'facealpha',0.5)
 ? ? ? ?end
 ? ? ? ?s=(num2str((i-1)*num_shange+j));
 ? ? ? ?text(j-0.95,i-0.5,s,'fontsize',6)
 ? ?end


end
axis([0 num_shange 0 num_shange])%限制图的边界
plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点
plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点
set(gca,'YDir','reverse');%图像翻转
for i=1:num_shange
 ? ?plot([0 num_shange],[i-1 i-1],'k-');
 ? ?plot([i i],[0 num_shange],'k-');%画网格线
end
for i=2:index1
 ? ?Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];
 ? ?Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];
 ? ?plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3)
end
title('粒子群-遗传算法-最优路线');
?
?
%进化曲线
figure(4);
plot(BestFitness);
xlabel('迭代次数')
ylabel('适应度值')
grid on;
title('粒子群-遗传算法-进化曲线');
disp('粒子群-遗传算法-最优路线方案：')
disp(num2str(route_lin))
disp(['起点到终点的距离:',num2str(BestFitness(end))]);