【路径规划】基于粒子群算法实现机器人栅格地图路径规划matlab源码

1 粒子群算法

模型参考这里。

3 代码

%% 粒子群clc;close allclearload(data4.mat)figure(1)%画障碍图hold onS=(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)==1y=[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) endendaxis([0 num_shange 0 num_shange])%限制图的边界plot(S_coo(2),S_coo(1), p,markersize, 10,