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

 一、和声搜索算法介绍

和声搜索（Harmony Search, HS）算法是一种新颖的智能优化算法。类似于遗传算法对生物进化的模仿、模拟退火算法对物理退火的模拟以及粒子群优化算法对鸟群的模仿等，和声算法模拟了音乐演奏的原理。

上图是一个由7个人组成的乐队，每个人演奏一种乐器，它们的演奏加起来对应一组和声X={x1, x2, x3, x4, x5, x6, x7}，他们会进行不断的进行配合以及排练来得到最好的和声效果，整个过程使用一个f(x)函数来衡量和声的效果好坏，这个f(x)就相当于总指挥，对他们演奏出的每一组和声进行权衡，如果达不到要求就继续演奏，直到得到一组满意的和声为止，这就是和声算法的最优化过程。

上面就是和声算法的基本思想，下面来具体看看算法的实现过程。

1.1、理解几个变量

1、和声库大小（ Harmony memory Size，HMS）：因为每个乐器演奏的音乐具有一定的范围，我们可以通过每个乐器的音乐演奏范围来构造一个解空间，然后通过这个解空间来随机产生一个和声记忆库，所以我们需要先为这个和声记忆库指定一个大小。

2、记忆库取值概率（Harmony memory considering rate, HMCR）：每次我们需要通过一定的概率来从这个和声记忆库中取一组和声，并且对这组和声进行微调，得到一个组新的和声，然后对这组新和声进行判别是否优于和声记忆库中最差的和声，这个判别使用的就是上面说的f(x)函数进行比较，所以，我们需要随机产生一个记忆库取值概率。

3、微调概率(Pitch adjusting rate, PAR)：上面已经说过，我们以一定的概率来在和声记忆库中选取一组和声，进行微调，这里指定的就是这个微调概率。

4、音调微调带宽 bw：上面说了会把从记忆库中取出的一组和声以一定的概率进行微调，这里指定就是这个调整幅度。

5、创作的次数 Tmax：这里指定的就是演奏家需要创作的次数，也就是上面整个调整过程需要重复多少次。

1.2、算法步骤

1、初始化上面的五个变量

2、确定解空间
上面有7个乐器，每个乐器的乐器演奏都在一定的范围内，通过确定每个乐器的音乐演奏范围，确定一个解空间。

上面的N对应的就是7，表示有7种乐器，U表示音乐的上限，L表示音乐的下限，这样就为每个乐器的乐器确定了一个范围，整个组合对应的就是一个解空间。

3、初始化和声记忆库

我们首先会根据初始化的和声库大小HMCR和上面和声的解空间来随机的产生一个大小为HMCR的和声记忆库。

矩阵表示如下：

4、产生一个新和声

（1）在[0，1]之间随机的产生一个变量rand1，并且将rand1与上面初始化的HMCR进行比较。

• 如果rand1小于HMCR，那么在上面初始化的和声记忆库中随机的得到一组和声
• 否则就在上面的解空间中随机的得到一组和声。

最终，就得到一组和声，如果这组和声是从和声库中得到的，就需要对这组和声进行微调，在[0，1]之间随机的产生一个变量rand2

• 如果rand2小于上面初始化的PAR，就需要以上面初始化的微调带宽bw来对和声进行调整，得到一组新和声
• 否则，不做调整

可以看到上面进行了两次的变异过程，最终得到了一组和声。

5、更新和声库

将得到的新和声使用f(x)进行进行计算，如果这个新和声比上面初始化的和声库HM中最差的的解具有更好的目标函数解，那么将这新的和声替换掉和声库中那个最差的和声。

6、判断是否终止

根据上面初始化的创作的次数来看当前创作次数是否已经达到这个最大次数，如果没有，则重复4-5的过程，直到达到最大创作次数。

三、栅格地图介绍

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

室内环境栅格法建模步骤

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
%% 和声算法
HMCR=0.95;%记忆库取值概
PAR=0.4;%微调概率
bw=0.1;%带宽
%%

%% 初始化路径
Group=ones(num_point,PopSize);  %种群初始化


%最优解
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))]);