基于栅格地图——遗传算法的机器人最优路径规划

采用栅格对机器人的工作空间进行划分，再利用优化算法对机器人路径优化，是采用智能算法求最优路径的一个经典问题。目前，采用蚁群算法在栅格地图上进行路径优化取得比较好的效果，而利用遗传算法在栅格地图上进行路径优化在算法显得更加难以实现。

利用遗传算法处理栅格地图的机器人路径规划的难点主要包括：1保证路径不间断，2保证路径不穿过障碍。

用遗传算法解决优化问题时的步骤是固定的，就是种群初始化，选择，交叉，变异，适应度计算这样，那么下面我就说一下遗传算法求栅格地图中机器人路径规划在每个步骤的问题、难点以及解决办法。

更多内容可查看我的博文：http://blog.sina.com.cn/s/blog_15e199e600102yxfn.html

1、种群初始化

首先要知道遗传算法种群初始化的定义。遗传算法种群初始化是生成一定种群数量的个体，每个个体是一个可行解。这里要注意是可行解，对于该问题也就是说是一个可行路径，也就是说应该是一个从路径起点到路径终点的，且不经过障碍的路径。怎样进行初始化种群得到可行路径是遗传算法求栅格路径的第一个难点也是最大难点。

方法

路径初始化可以分为两步：第一步生成必经节点路径，什么是必经节点路径？举个例子，比如对于10*10的栅格，从左下角编号1到右上角编号100的路径必经过第2行的一个点，第3行一个点……第9行的一个点，这是很显然的，那么我们在第二行的自由栅格中随机取一个节点、第3行的自由栅格中取一个节点……第9行自由栅格取一个节点，那么这就行程了一个必经点路径，当然这个路径也是间断的。所以需要第二步，第二步就是连接这些间断节点，而这个问题是该算法中最难的问题，因为在连接路径中，太难绕开障碍了，而且如果你绕开了障碍物，会发现失去了方向连不回来了。因此，在连接节点中采用中点连接法，但是中点，要取得有技巧，可在中点处取4或3个栅格，然后在这些栅格中找到自由栅格，等概率选择，如果有最坏得情形，这些中点处栅格全都是障碍，则在这些栅格中等概率选择一个作为路径一点，因此该方法保证了路径的连续性，但也有可能存在经过障碍的路径，而这种障碍的路径可以在适应度函数中进行惩罚。

上述初始化路径的两步结束后，进行简化路径操作，即如果路径中有两个相同的节点，则去掉相同节点之间的一段，这个很容易理解。

至此初始化路径结束，你已经成功了一大步！

 

2、选择

选择和遗传算法的选择是一样的，无需特殊改动，就是根据适应度进行选择。

3、交叉

交叉采用重复点交叉，这个也比较好理解，比如一条路径为[1 12 13 24 25 36 45 56 ]，另一条路径为[ 1 14 25 35 46 56]，这两条路径有一个公共节点25，进行交叉变成[1 12 13 24 25 35 46 56]和[1 14 25 36 45 56].

4、变异

   变异的方法使用随机生成选择两个节点，并去掉这两个节点之间的路径，之后采用上述的中带你插入方法生成连续路径。

5、适应度计算

适应度计算路径长度和穿过障碍的个数，求和即可，这个比较间断不多说，下面展示一下我写的程序的效果。

20*20栅格的

http://s9/mw690/006pvsS4zy7h7mAQYfSe8&690

10*10栅格的

 

http://s6/mw690/006pvsS4zy7h7mCuaSF55&690

本程序包括以下所有文件

http://s3/mw690/006pvsS4zy7h7mO3l06e2&690

%%%% 基于遗传算法的栅格法机器人路径规划

%%%% by 圆创工作室  QQ530807088

%%%% 特别声明：本路径规划方法是本人原创方法，具体实现方式请参考附件。

clc

clear

close all

%% 输入数据

G=  [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;

    0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;

    0 0 1 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 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 1 1 1 0 0 0 0 0 0 0 0 0 0 0;

    0 0 0 0 0 0 1 1 1 0 1 0 1 1 0 0 0 0 0 0;

    0 1 1 1 0 0 0 0 0 0 1 0 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 0 1 1 0 1 1 1 1 0 0 0 0 0 0;

    0 0 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 0 0 0 0 1 1 1 0 1 1 1 1 0;

    0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;

    0 0 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0;

    0 0 1 1 0 0 1 1 1 0 1 0 0 0 0 0 0 0 0 0;

    0 0 0 0 0 0 1 1 1 0 1 0 0 0 0 0 0 1 1 0;

    0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 0 0 1 1 0;

    0 0 0 0 0 0 0 0 0 0 0 0 0 0 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];

Pstart=0;    % 起始序号

Pend=399;    % 终止序号

NP=200;      % 种群数量

maxgen=200;  % 最大进化代数

Pc=0.8;      % 交叉概率

Pm=0.2;      % 变异概率

Gap=0.9;     % 代沟(Generation gap)

%% 栅格绘制

drawShanGe(G,0)

%%

[yGrid,xGrid]=size(G);

xs=rem(Pstart,xGrid)+1;   % 起点所在列（从左到右编号列1.2.3...）

ys=fix(Pstart/yGrid)+1;   % 起点所在行（从上到下编号行1.2.3...）

xe=rem(Pend,xGrid)+1;     % 终点所在列

ye=fix(Pend/yGrid)+1;     % 终点所在行

%% 种群初始化-step1，必经节点,从起始点所在行开始往上，在每行中挑选一个自由栅格，构成必经节点

PassNum=ye-ys+1;

R=zeros(NP,PassNum);

for i=1:NP

    R(i,1)=Pstart;

    j=1;

    for yk=ys+1:ye-1

        j=j+1;

        Can=[]; % 每一行的可选节点

        for xk=1:xGrid

            % 栅格序号

            No=(xk-1)+(yk-1)*xGrid;

            if G(yk,xk)==0

                Can=[Can No];

            end

        end

        CanNum=length(Can);

        index=randi(CanNum);

        R(i,j)=Can(index);

    end

    R(i,end)=Pend;

    

    %% 种群初始化-step连接节点：据中间在中间节点

    % 将上述必经节点联结成无间断路径

    route=generateContinuousRoute(R(i,:),G);

    route=shortenRoute(route);    % 简化路径

    X(i,1)={route};

end

% 上述初始化路径存在路径经过障碍

for i=1:NP

    route=X{i,1};

    fx(i)=fitness(route,G);  % 带惩罚的路径长度

end

[fgbest,indexmin]=min(fx);

gbest=X{indexmin,1};

gen=1;

while gen

    

    % 计算适应度

    fit=1./(fx+1);

    % 选择

    XSel=Select(X,fit,Gap);

    % 交叉操作

    XSel=Recombin(XSel,Pc);

    % 变异

    XSel=Mutate(XSel,Pm,G);

    % 逆转操作---------可以将该函数去掉

    XSel=Reverse(XSel,G);

    % 重插入子代的新种群

    X=Reins(X,XSel,fit);

    for i=1:NP

        route=X{i,1};

        route=shortenRoute(route);

        X(i,1)={route};

        fx(i)=fitness(route,G);  % 带惩罚的路径长度

    end

    % 记录各代最优值

    [fpbest,indexmin]=min(fx);

    FG(gen)=fpbest;       % 各代最短路径

    XG(gen,1)=X(indexmin,1);

    if fpbest

        fgbest=fpbest;

        gbest=X{indexmin,1};

    end

    % 更新迭代次数

    gen=gen+1 ;

end

%%

disp('最佳路径：')

gbest

disp('最优距离为：')

fgbest

%% 绘图

figure

drawPath(gbest,G,0)

figure

plot(FG)

更多内容可查看我的博文：http://blog.sina.com.cn/s/blog_15e199e600102yxfn.html