【路径规划】基于灰狼算法实现机器人栅格地图路径规划

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法 神经网络预测 雷达通信  无线传感器 电力系统

信号处理 图像处理 路径规划 元胞自动机 无人机 

⛄ 内容介绍

一、灰狼算法介绍

灰狼优化算法（Grey Wolf Optimizer，GWO）由澳大利亚格里菲斯大学学者 Mirjalili 等人于2014年提出来的一种群智能优化算法。该算法受到了灰狼捕食猎物活动的启发而开发的一种优化搜索方法，它具有较强的收敛性能、参数少、易实现等特点。近年来受到了学者的广泛关注，它己被成功地应用到了车间调度、参数优化、图像分类等领域中。

---

算法原理：

灰狼隶属于群居生活的犬科动物，且处于食物链的顶层。灰狼严格遵守着一个社会支配等级关系。如图：

社会等级第一层：狼群中的头狼记为 ， 狼主要负责对捕食、栖息、作息时间等活动作出决策。由于其它的狼需要服从 狼的命令，所以 狼也被称为支配狼。另外， 狼不一定是狼群中最强的狼，但就管理能力方面来说， 狼一定是最好的。

社会等级第二层： 狼，它服从于 狼，并协助 狼作出决策。在 狼去世或衰老后， 狼将成为 狼的最候选者。虽然 狼服从 狼，但 狼可支配其它社会层级上的狼。

社会等级第三层： 狼，它服从 、 狼，同时支配剩余层级的狼。 狼一般由幼狼、哨兵狼、狩猎狼、老年狼及护理狼组成。

社会等级第四层： 狼，它通常需要服从其它社会层次上的狼。虽然看上去 狼在狼群中的作用不大，但是如果没有 狼的存在，狼群会出现内部问题如自相残杀。

GWO 优化过程包含了灰狼的社会等级分层、跟踪、包围和攻击猎物等步骤，其步骤具体情况如下所示。

１）社会等级分层（Social Hierarchy）当设计 GWO 时，首先需构建灰狼社会等级层次模型。计算种群每个个体的适应度，将狼群中适应度最好的三匹灰狼依次标记为 、 、 ，而剩下的灰狼标记为 。也就是说，灰狼群体中的社会等级从高往低排列依次为； 、 、 及 。GWO 的优化过程主要由每代种群中的最好三个解（即 、 、 ）来指导完成。

２）包围猎物（ Encircling Prey ）灰狼捜索猎物时会逐渐地接近猎物并包围它，该行为的数学模型如下：

式中：t 为当前迭代次数：。表示 hadamard 乘积操作；A 和 C 是协同系数向量；Xp 表示猎物的位置向量； X(t) 表示当前灰狼的位置向量；在整个迭代过程中 a 由2 线性降到 0； r1 和 r2 是 [0，1] 中的随机向量。

３）狩猎（ Hunring）

灰狼具有识别潜在猎物（最优解）位置的能力，搜索过程主要靠 、 、 灰狼的指引来完成。但是很多问题的解空间特征是未知的，灰狼是无法确定猎物（最优解）的精确位置。为了模拟灰狼（候选解）的搜索行为，假设 、 、 具有较强识别潜在猎物位置的能力。因此，在每次迭代过程中，保留当前种群中的最好三只灰狼（ 、 、 ），然后根据它们的位置信息来更新其它搜索代理（包括 ）的位置。该行为的数学模型可表示如下：

式中：、、 分别表示当前种群中 、 、 的位置向量；Ｘ表示灰狼的位置向量；、、 分别表示当前候选灰狼与最优三条狼之间的距离；当｜A｜＞１时，灰狼之间尽量分散在各区域并搜寻猎物。当｜A｜＜１时，灰狼将集中捜索某个或某些区域的猎物。

从图中可看出，候选解的位置最终落在被 、 、 定义的随机圆位置内。总的来说， 、 、 需首先预测出猎物（潜 在最优解）的大致位置，然后其它候选狼在当前最优兰只狼的指引下在猎物附近随机地更新它们的位置。

４）攻击猎物（Attacking Prey）构建攻击猎物模型的过程中，根据2)中的公式，ａ值的减少会引起 A 的值也随之波动。换句话说，A 是一个在区间［-a，a］（备注：原作者的第一篇论文里这里是[-2a,2a]，后面论文里纠正为[-a,a]）上的随机向量，其中ａ在迭代过程中呈线性下降。当 A 在［-1，１］区间上时，则捜索代理（Search Agent）的下一时刻位置可以在当前灰狼与猎物之间的任何位置上。

５）寻找猎物（Search for Prey）灰狼主要依赖 、 、 的信息来寻找猎物。它们开始分散地去搜索猎物位置信息，然后集中起来攻击猎物。对于分散模型的建立，通过｜A｜＞１使其捜索代理远离猎物，这种搜索方式使 GWO 能进行全局搜索。GWO 算法中的另一个搜索系数是Ｃ。从2)中的公式可知，Ｃ向量是在区间范围［０，２］上的随机值构成的向量，此系数为猎物提供了随机权重，以便増加（｜Ｃ｜＞１）或减少（｜Ｃ｜＜１）。这有助于 GWO 在优化过程中展示出随机搜索行为，以避免算法陷入局部最优。值得注意的是，Ｃ并不是线性下降的，Ｃ在迭代过程中是随机值，该系数有利于算法跳出局部，特别是算法在迭代的后期显得尤为重要。

二、栅格地图介绍

栅格地图有两种表示方法，直角坐标系法和序号法，序号法比直角坐标法节省内存

室内环境栅格法建模步骤

1.栅格粒大小的选取

栅格的大小是个关键因素，栅格选的小，环境分辨率较大，环境信息存储量大，决策速度慢。

栅格选的大，环境分辨率较小，环境信息存储量小，决策速度快，但在密集障碍物环境中发现路径的能力较弱。

2.障碍物栅格确定

当机器人新进入一个环境时，它是不知道室内障碍物信息的，这就需要机器人能够遍历整个环境，检测障碍物的位置，并根据障碍物位置找到对应栅格地图中的序号值，并对相应的栅格值进行修改。自由栅格为不包含障碍物的栅格赋值为0，障碍物栅格为包含障碍物的栅格赋值为1.

3.未知环境的栅格地图的建立

通常把终点设置为一个不能到达的点，比如（-1，-1），同时机器人在寻路过程中遵循“下右上左”的原则，即机器人先向下行走，当机器人前方遇到障碍物时，机器人转向右走，遵循这样的规则，机器人最终可以搜索出所有的可行路径，并且机器人最终将返回起始点。

备注：在栅格地图上，有这么一条原则，障碍物的大小永远等于n个栅格的大小，不会出现半个栅格这样的情况。

三、代码

clc;
close all
clear
load('data1.mat')
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);%终点对应的编号

PopSize=20;%种群大小
OldBestFitness=0;%旧的最优适应度值
gen=0;%迭代次数
maxgen =100;%最大迭代次数

c1=0.6;%头狼保留概率
c2=0.3;%次狼保留概率
c3=0.1;%差狼保留概率
%%
Alpha_score=inf; %change this to -inf for maximization problems
Beta_score=inf; %change this to -inf for maximization problems
Delta_score=inf; %change this to -inf for maximization problems
%Initialize the positions of search agents
%初始化路径
Group=ones(num_point,PopSize);  %种群初始化
flag=1;
%% 初始化粒子群位置


%最优解
figure(1)
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(2);
plot(BestFitness);
xlabel('迭代次数')
ylabel('适应度值')
grid on;
title('进化曲线');
disp('基础狼群算法-最优路线方案：')
disp(num2str(route_lin))
disp(['起点到终点的距离:',num2str(BestFitness(end))]);