【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】

原创

佛怒火莲 2021-11-08 13:40:01 博主文章分类：matlab完整代码 ©著作权

文章标签 栅格最优解邻接矩阵极值粒子群 文章分类 数据结构与算法人工智能

©著作权归作者所有：来自51CTO博客作者佛怒火莲的原创作品，请联系作者获取转载授权，否则将追究法律责任

一、粒子群及栅格地图简介

1 粒子群算法
1.1 粒子群算法的概念
粒子群优化算法(PSO：Particle swarm optimization) 是一种进化计算技术（evolutionary computation）。源于对鸟群捕食的行为研究。粒子群优化算法的基本思想：是通过群体中个体之间的协作和信息共享来寻找最优解．
PSO的优势：在于简单容易实现并且没有许多参数的调节。目前已被广泛应用于函数优化、神经网络训练、模糊系统控制以及其他遗传算法的应用领域。

1.2 粒子群算法分析
1.2.1基本思想
粒子群算法通过设计一种无质量的粒子来模拟鸟群中的鸟，粒子仅具有两个属性：速度和位置，速度代表移动的快慢，位置代表移动的方向。每个粒子在搜索空间中单独的搜寻最优解，并将其记为当前个体极值，并将个体极值与整个粒子群里的其他粒子共享，找到最优的那个个体极值作为整个粒子群的当前全局最优解，粒子群中的所有粒子根据自己找到的当前个体极值和整个粒子群共享的当前全局最优解来调整自己的速度和位置。下面的动图很形象地展示了PSO算法的过程：
【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】_粒子群
1.2.2 更新规则
PSO初始化为一群随机粒子(随机解)。然后通过迭代找到最优解。在每一次的迭代中，粒子通过跟踪两个“极值”(pbest，gbest)来更新自己。在找到这两个最优值后，粒子通过下面的公式来更新自己的速度和位置。
【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】_极值_02
公式(1)的第一部分称为【记忆项】，表示上次速度大小和方向的影响；公式(1)的第二部分称为【自身认知项】，是从当前点指向粒子自身最好点的一个矢量，表示粒子的动作来源于自己经验的部分；公式(1)的第三部分称为【群体认知项】，是一个从当前点指向种群最好点的矢量，反映了粒子间的协同合作和知识共享。粒子就是通过自己的经验和同伴中最好的经验来决定下一步的运动。以上面两个公式为基础，形成了PSO的标准形式。
【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】_邻接矩阵_03
公式(2)和公式(3)被视为标准PSO算法。
1.2.3 PSO算法的流程和伪代码

2 栅格地图
2.1 栅格法应用背景
路径规划时首先要获取环境信息，建立环境地图，合理的环境表示有利于建立规划方法和选择合适的搜索算法，最终实现较少的时间开销而规划出较为满意的路径。一般使用栅格法在静态环境下建立环境地图。
2.2 栅格法实质
将AGV的工作环境进行单元分割，将其用大小相等的方块表示出来，这样栅格大小的选取是影响规划算法性能的一个很重要的因素。栅格较小的话，由栅格地图所表示的环境信息将会非常清晰，但由于需要存储较多的信息，会增大存储开销，同时干扰信号也会随之增加，规划速度会相应降低，实时性得不到保证；反之，由于信息存储量少，抗干扰能力有所增强，规划速随之增快，但环境信息划分会变得较为模糊，不利于有效路径的规划。在描述环境信息时障碍物所在区域在栅格地图中呈现为黑色，地图矩阵中标为1，可自由通行区域在栅格地图中呈现为白色，地图矩阵中标为0。路径规划的目的就是在建立好的环境地图中找到一条最优的可通行路径，所以使用栅格法建立环境地图时，栅格大小的合理设定非常关键。
2.3 10乘10的静态环境地图
【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】_最优解_05
10乘10的静态环境地图代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境地图%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DrawMap(map)
n = size(map);
step = 1;
a = 0 : step :n(1);
b = 0 : step :n(2);
figure(1)
axis([0 n(2) 0 n(1)]); %设置地图横纵尺寸
set(gca,'xtick',b,'ytick',a,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
hold on
r = 1;
for(i=1:n(1))         %设置障碍物的左下角点的x,y坐标
    for(j=1:n(2))
        if(map(i,j)==1)
            p(r,1)=j-1;
            p(r,2)=i-1;
            fill([p(r,1) p(r,1) + step p(r,1) + step p(r,1)],...
                 [p(r,2) p(r,2) p(r,2) + step p(r,2) + step ],'k');
            r=r+1;
            hold on
        end
    end
end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%栅格数字标识%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_text = 1:1:n(1)*n(2); %产生所需数值.
for i = 1:1:n(1)*n(2)
    [row,col] = ind2sub([n(2),n(1)],i);
    text(row-0.9,col-0.5,num2str(x_text(i)),'FontSize',8,'Color','0.7 0.7 0.7');
end
hold on
axis square

建立环境矩阵，1代表黑色栅格，0代表白色栅格，调用以上程序，即可得到上述环境地图。

map=[0 0 0 1 0 0 1 0 0 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 1 0 0 0 1 1 0 0;
     0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 1 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 0 1 0 0 0 0 0 0;
     1 1 1 0 0 0 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;];
     DrawMap(map);         %得到环境地图

2.4 栅格地图中障碍栅格处路径约束
移动体栅格环境中多采用八方向的移动方式，此移动方式在完全可通行区域不存在运行安全问题，当
移动体周围存在障碍栅格时此移动方式可能会发生与障碍物栅格的碰撞问题，为解决此问题加入约束
条件，当在分别与障碍物栅格水平方向和垂直方向的可行栅格两栅格之间通行时，禁止移动体采用对
角式移动方式。
【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】_栅格_06

约束条件的加入，实质是改变栅格地图的邻接矩阵，将障碍栅格(数字为“1”的矩阵元素)的对角栅格
设为不可达，即将对角栅格的距离值改为无穷大。其实现MATLAB代码如下：
代码：

%约束移动体在障碍栅格对角运动
%通过优化邻接矩阵实现
%%%%%%%%%%%%%%%%%% 约束移动体移动方式 %%%%%%%%%%%%%%%%%
function W=OPW(map,W)
% map 地图矩阵  % W 邻接矩阵
n = size(map);
num = n(1)*n(2);
for(j=1:n(1))
    for(z=1:n(2))
       if(map(j,z)==1)
          if(j==1)                  %若障碍物在第一行
             if(z==1)               %若障碍物为第一行的第一个
                W(j+1,j+n(2)*j)=Inf;
                W(j+n(2)*j,j+1)=Inf;
             else
                if(z==n(2))         %若障碍物为第一行的最后一个
                   W(n(2)-1,n(2)+n(1)*j)=Inf;
                   W(n(2)+n(1)*j,n(2)-1)=Inf;
                else                %若障碍物为第一行的其他
                    W(z-1,z+j*n(2))=Inf;
                    W(z+j*n(2),z-1)=Inf;
                    W(z+1,z+j*n(2))=Inf;
                    W(z+j*n(2),z+1)=Inf;
                end
             end
          end
          if(j==n(1))               %若障碍物在最后一行
             if(z==1)               %若障碍物为最后一行的第一个
                W(z+n(2)*(j-2),z+n(2)*(j-1)+1)=Inf;
                W(z+n(2)*(j-1)+1,z+n(2)*(j-2))=Inf;
             else
             if(z==n(2))            %若障碍物为最后一行的最后一个
                W(n(1)*n(2)-1,(n(1)-1)*n(2))=Inf;
                W((n(1)-1)*n(2),n(1)*n(2)-1)=Inf;
             else                   %若障碍物为最后一行的其他
                W((j-2)*n(2)+z,(j-1)*n(2)+z-1)=Inf;
                W((j-1)*n(2)+z-1,(j-2)*n(2)+z)=Inf;
                W((j-2)*n(2)+z,(j-1)*n(2)+z+1)=Inf;
                W((j-1)*n(2)+z+1,(j-2)*n(2)+z)=Inf;
             end
             end
          end
          if(z==1)              
             if(j~=1&&j~=n(1))       %若障碍物在第一列非边缘位置 
                W(z+(j-2)*n(2),z+1+(j-1)*n(2))=Inf;
                W(z+1+(j-1)*n(2),z+(j-2)*n(2))=Inf;
                W(z+1+(j-1)*n(2),z+j*n(2))=Inf;
                W(z+j*n(2),z+1+(j-1)*n(2))=Inf;
             end
          end
         if(z==n(2))
            if(j~=1&&j~=n(1))         %若障碍物在最后一列非边缘位置 
               W((j+1)*n(2),j*n(2)-1)=Inf;
               W(j*n(2)-1,(j+1)*n(2))=Inf;
               W(j*n(2)-1,(j-1)*n(2))=Inf;
               W((j-1)*n(2),j*n(2)-1)=Inf;
            end
         end
         if(j~=1&&j~=n(1)&&z~=1&&z~=n(2))   %若障碍物在非边缘位置
            W(z+(j-1)*n(2)-1,z+j*n(2))=Inf;
            W(z+j*n(2),z+(j-1)*n(2)-1)=Inf;
            W(z+j*n(2),z+(j-1)*n(2)+1)=Inf;
            W(z+(j-1)*n(2)+1,z+j*n(2))=Inf;
            W(z+(j-1)*n(2)-1,z+(j-2)*n(2))=Inf;
            W(z+(j-2)*n(2),z+(j-1)*n(2)-1)=Inf;
            W(z+(j-2)*n(2),z+(j-1)*n(2)+1)=Inf;
            W(z+(j-1)*n(2)+1,z+(j-2)*n(2))=Inf;
         end
       end
     end
   end
end

2.5 栅格法案例
下面以Djkstra算法为例，其实现如下：

map=[0 0 0 1 0 0 1 0 0 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 1 0 0 0 1 1 0 0;
     0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 1 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 0 1 0 0 0 0 0 0;
     1 1 1 0 0 0 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境矩阵map%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DrawMap(map); %得到环境地图
W=G2D(map);   %得到环境地图的邻接矩阵
W(W==0)=Inf;  %邻接矩阵数值处理
W=OPW(map,W); %优化邻接矩阵
[distance,path]=dijkstra(W,1,100);%设置起始栅格，得到最短路径距离以及栅格路径
[x,y]=Get_xy(distance,path,map);   %得到栅格相应的x，y坐标
Plot(distance,x,y);   %画出路径

运行结果如下：
【路径规划】基于matlab粒子群算法栅格地图路径规划【含Matlab源码 579期】_邻接矩阵_08
其中函数程序：
DrawMap(map) 详见建立栅格地图
W=G2D(map) ；详见建立邻接矩阵
[distance， path] =dijkstra(W， 1， 100) 详见Djk stra算法
[x， y] =Get_xy(distance， path， map) ；
Plot(distance， x， y) ；

二、部分源代码

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 =20;%最大迭代次数

c1=0.5;%认知系数
c2=0.7;%社会学习系数
w=0.96;%惯性系数
%%
%初始化路径
w_min=0.5;
w_max=1;
Group=ones(num_point,PopSize);  %种群初始化
for i=1:PopSize
    p_lin=randperm(num_point)';%随机生成1*400不重复的行向量
    %% 将起点编号放在首位
    index=find(p_lin==S);
    lin=p_lin(1);
    p_lin(1)=p_lin(index);
    p_lin(index)=lin;
    Group(:,i)=p_lin;
    %%将每个个体进行合理化处理
    [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);
    while flag==1%如处理不成功，则初始化个体，重新处理
        p_lin=randperm(num_point)';
        index=find(p_lin==S);
        lin=p_lin(1);
        p_lin(1)=p_lin(index);
        p_lin(index)=lin;
        Group(:,i)=p_lin;
        [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);       
    end
end

%最优解
route=Group(:,end)';
index1=find(route==E);
route_lin=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)],'b-.','LineWidth',3);hold on
    
end
title('粒子群算法-随机路线');

title('粒子群算法-随机路线');
figure(2)
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)],'b-.','LineWidth',3)
end
%初始化粒子速度（即交换序）
Velocity =zeros(num_point,PopSize);   
for i=1:PopSize
    Velocity(:,i)=round(rand(1,num_point)'*num_point/10); %round取整
end

%计算每个个体对应路径的距离
for i=1:PopSize   
	EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);
end

IndivdualBest=Group;%记录各粒子的个体极值点位置,即个体找到的最短路径
IndivdualBestFitness=EachPathDis;%记录最佳适应度值,即个体找到的最短路径的长度
[GlobalBestFitness,index]=min(EachPathDis);%找出全局最优值和相应序号
%寻优
while gen < maxgen
    w=w_max-(w_max-w_min)*gen/maxgen;
    %迭代次数递增
    gen = gen +1
    %更新全局极值点位置,这里指路径
    for i=1:PopSize   
        GlobalBest(:,i) = Group(:,index);
    end
   
    %求pij-xij ,pgj-xij交换序，并以概率c1，c2的保留交换序
    pij_xij=GenerateChangeNums(Group,IndivdualBest);  %根据个体最优解求交换序
    pij_xij=HoldByOdds(pij_xij,c1);%以概率c1保留交换序
    pgj_xij=GenerateChangeNums(Group,GlobalBest);%根据全局最优解求交换序
    pgj_xij=HoldByOdds(pgj_xij,c2);%以概率c2保留交换序
   
    %以概率w保留上一代交换序
    Velocity=HoldByOdds(Velocity,w);
    Group = PathExchange(Group,Velocity); %根据交换序进行路径交换
    Group = PathExchange(Group,pij_xij);
    Group = PathExchange(Group,pgj_xij);
    for i = 1:PopSize    
        [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);
        while flag==1
            p_lin=randperm(num_point)';
            index=find(p_lin==S);
            lin=p_lin(1);
            p_lin(1)=p_lin(index);
            p_lin(index)=lin;
            Group(:,i)=p_lin;
            [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);
        end    
    end 
    for i = 1:PopSize    % 更新各路径总距离
        EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);
    end
    IsChange = EachPathDis<IndivdualBestFitness;%更新后的距离优于更新前的，记录序号
    IndivdualBest(:, find(IsChange)) = Group(:, find(IsChange));%更新个体最佳路径
    IndivdualBestFitness = IndivdualBestFitness.*( ~IsChange) + EachPathDis.*IsChange;%更新个体最佳路径距离
    [GlobalBestFitness, index] = min(IndivdualBestFitness);%更新全局最佳路径,记录相应的序号

    if GlobalBestFitness~=OldBestFitness %比较更新前和更新后的适应度值;
        OldBestFitness=GlobalBestFitness;%不相等时更新适应度值
        best_route=IndivdualBest(:,index)';
    end   
     BestFitness(gen) =GlobalBestFitness;%每一代的最优适应度
end
%最优解
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