【路径规划】基于matlab麻雀算法求解机器人栅格地图最短路径规划问题【含Matlab源码 1582期】

原创

佛怒火莲 2021-12-10 11:41:15 博主文章分类：matlab完整代码 ©著作权

文章标签 matlab 算法开发语言栅格移动机器人 文章分类 数据结构与算法人工智能

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

一、简介

路径规划是实现移动机器人自主导航的关键技术，是指在有障碍物的环境中，按照一定的评价标准（如距离、时间、能耗等），寻找到一条从起始点到目标点的无碰撞路径，这里选取最短距离路径规划的评价标准，即最短路径规划问题。

1．路径规划数学模型的建立

将移动机器人周围环境用一组数据进行抽象表达，建立二维或三维的环境模型，得到移动机器人能够理解分析的环境数据，是机器人路径规划的基本前提。我这里用的是栅格法，其原理是将周围环境看成一个二维平面，将平面分成一个个等面积大小的具有二值信息的栅格，每个栅格中存储着周围环境信息量，下图我给出了一个栅格法地图，方便大家更好的理解栅格地图。这里设计的栅格地图为一个20×20的地形矩阵，黑色的地方表示有障碍，白色的地方表示没有障碍。

【路径规划】基于matlab麻雀算法求解机器人栅格地图最短路径规划问题【含Matlab源码 1582期】_算法

图1　栅格法地图

在用栅格法建立环境模型时，为了将环境信息转换成移动机器人可以识别的数据，一般采用序号法标记环境地图信息，即将栅格地图中一个个栅格从序号１依次累加直到标记到最后一个栅格。如图2所示。

【路径规划】基于matlab麻雀算法求解机器人栅格地图最短路径规划问题【含Matlab源码 1582期】_算法_02

【路径规划】基于matlab麻雀算法求解机器人栅格地图最短路径规划问题【含Matlab源码 1582期】_算法_03

图3　八叉树搜索策略

那么，怎么判断一个栅格点是否为另一个栅格点的相邻栅格点呢，另外，又怎么判断是否为有障碍栅格呢。这就需建立矩阵D，记录每个栅格点至其相邻栅格点的代价值。本例中栅格地图有20×20个栅格点，则D的大小为400×400，其中列是起点栅格，行是局部终点栅格，各栅格点至其各相邻无障碍栅格点的代价值非零，而有障碍栅格及非相邻栅格设为0。

二、部分源代码

clc
clear
close all
tic
%% 地图
G=[0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 
   0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
   0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   1 1 1 1 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   1 1 1 1 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 0 1 1 1 0 0 0 0 0 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; 
   0 0 1 1 0 0 0 0 0 0 1 1 0 0 1 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;];
for i=1:20/2
    for j=1:20
        m=G(i,j);
        n=G(21-i,j);
        G(i,j)=n;
        G(21-i,j)=m;
    end
end
%% 
S = [1 1];   
E = [20 20];  
G0 = G;
G = G0(S(1):E(1),S(2):E(2)); 
[Xmax,dimensions] = size(G);        
dimensions = dimensions - 2;             

%% 参数设置
max_gen = 200;    % 最大迭代次数
num_polution = 50;         % 种群数量
soft_value = 0.8;        %安全值
recover_Percent = 0.3;  %%发现者比例
scout_Percent = 0.2;  %%侦查者比例
recover_Num = round( num_polution * recover_Percent );    % %发现者
scout_Num = round(num_polution * scout_Percent);      %%侦查者数量
X_min = 1;  

%% 初始化
X = zeros(num_polution,dimensions);
for i = 1:num_polution
    for j = 1:dimensions
       column = G(:,j+1);      % 地图的一列
       id = find(column == 0); % 该列自由栅格的位置
       X(i,j) =  id(randi(length(id))); % 随机选择一个自由栅格
       id = [];
    end 
    fit( i ) = fitness(X( i, : ),G);%%%行向量
end
fit_person_best = fit;   % 个体最优适应度
person_best = X;      % 个体最优位置
[fit_global_best, best_person_Index] = min( fit );        % 全局最优适应度
global_best = X(best_person_Index, : );    % 全局最优位置
[fit_max,B]=max(fit);
worse_person= X(B,:);  
%%
for gene = 1:max_gen
    gene
    [ans1,sort_Index] = sort(fit);   %适应值度从小到大排序
    [fit_max,B] = max(fit);
    worse_person = X(B,:);           %找出适应度最差的个体
    [~,Index] = sort(fit_person_best);
    r2 = rand(1);
    if r2 < soft_value

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for i = 1:recover_Num                                                  
            r1 = rand(1);  
            X(Index(i),:) = person_best(Index(i),:)*exp(-(i)/(r1*max_gen));         %%%%%发现者广泛进行搜索操作
            X(Index(i),:) = Bounds(X(Index(i),:), X_min,Xmax);                      %%%%输出整数路径
            fit(Index(i)) = fitness(X(Index(i),:),G);
        end
    else
            X(Index(i),:) = person_best(Index(i),:) + randn(1)*ones(1,dimensions);  %%%%%%%飞往其他地方觅食
            X(Index(i),:) = Bounds(X(Index(i),:),X_min,Xmax);    
            fit(Index(i)) = fitness(X(Index(i),:),G);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%发现者%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [fit_MMin,best_best] = min(fit);      
    best_best_bt = X(best_best,:);     
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for i = (recover_Num+1):num_polution                   
        A = floor(rand(1,dimensions)*2)*2-1;                %%%%%%%%-1到1的随机数
        if(i>(num_polution/2))
            X(Index(i),:) = randn(1)*exp((worse_person-person_best(Index(i),:))/(i)^2);%%%%%%%适应度较低的个体没有获得食物，将飞往其他地方随机范围
        else
            X(Index(i),:) = best_best_bt+(abs((person_best(Index(i),:)-best_best_bt)))*(A'*(A*A')^(-1))*ones(1,dimensions); %%%%%%%中等适应度值的个体更新位置
        end
        X(Index(i),:) = Bounds(X(Index(i),:),X_min,Xmax);
        fit(Index(i)) = fitness(X(Index(i),:),G);
    end
    p_1 = randperm(numel(sort_Index));
    p_2 = sort_Index(p_1(1:scout_Num));
    for j = 1:length(p_2)     
        if(fit_person_best(Index(p_2(j)))>(fit_global_best))
            X(Index(p_2(j)),:) = global_best+(randn(1,dimensions)).*(abs((person_best(Index( p_2(j)),:) -global_best)));%%%%%%%%意识到危险的麻雀向种群适应度最高的个体靠拢
        else
            X(Index(p_2(j)),:) = person_best(Index(p_2(j)),:)+(2*rand(1)-1)*(person_best(Index(p_2(j)),:)-worse_person)/( fit_person_best(Index(p_2(j)))-fit_max+1e-50);
        end
        X(Index(j),:) = Bounds(X(Index(j),:), X_min, Xmax);
        fit(Index(j)) = fitness(X(Index(j),:),G);
    end
     for i=1:num_polution
         X(i,:) = Bounds(X(i,:),X_min,Xmax);
     end
    % 更新个体最优值和全局最优值
    for i = 1:num_polution

        end
        if(fit_person_best(i) < fit_global_best)
            fit_global_best = fit_person_best(i);
            global_best = person_best(i,:);
        end
    end
    global_best = LocalSearch(global_best,Xmax,G);%%%%%把全局最优解进行局部搜索，提高全局最优解适应度值
    fit_global_best = fitness(global_best,G);
    final_goal(gene,1)=fit_global_best;
end
function fx = fitness(x,G)
    S = [1 1];       % 新地图的起点
    E = size(G);     % 新地图的终点
    route = [S(1) x E(1)];
    dim = length(route);
    nB = 0;        % 粒子路径是否经过障碍的数目
    route=round(route);
    for j = 2 : dim-1
       if G(route(j),j) == 1
           nB = nB + 1;
       end
    end
    if nB == 0     % 
        path=generateContinuousRoute(route,G); % 中点邻域搜索
%         path=shortenRoute(path);
        path=GenerateSmoothPath(path,G);
        path=GenerateSmoothPath(path,G);
        fx = 0;
        for i = 1:size(path,1)-1
            fx = fx + sqrt((path(i+1,1)-path(i,1))^2 + (path(i+1,2)-path(i,2))^2);
        end
    else
    fx = E(1)*E(2) * nB;
    end