算法的思路如下:取各障碍物顶点连线的中点为路径点,相互连接各路径点,将机器人移动的起点和终点限制在各路径点上,利用Dijkstra算法来求网络图的最短路径,找到从起点P1到终点Pn的最短路径,由于上述算法使用了连接线中点的条件,不是整个规划空间的最优路径,然后利用遗传算法对找到的最短路径各个路径点Pi (i=1,2,…n)调整,让各路径点在相应障碍物端点连线上滑动,利用Pi= Pi1+ti×(Pi2-Pi1)(ti∈[0,1] i=1,2,…n)即可确定相应的Pi,即为新的路径点,连接此路径点为最优路径。本源码由GreenSim团队原创,转载请注明,有意购买源码或代写相关程序,请与GreenSim团队联系(主页http://blog.sina.com.cn/greensim)


function [L1,XY1,L2,XY2]=JQRLJGH(XX,YY)
%% 基于Dijkstra和遗传算法的机器人路径规划演示程序
% GreenSim团队原创作品,转载请注明
% Email:greensim@163.com
% GreenSim团队主页:http://blog.sina.com.cn/greensim
% [color=red]欢迎访问GreenSim——算法仿真团队→[url=http://blog.sina.com.cn/greensim]http://blog.sina.com.cn/greensim[/url][/color]
%输入参数在函数体内部定义
%输出参数为
%  L1    由Dijkstra算法得出的最短路径长度
%  XY1   由Dijkstra算法得出的最短路径经过节点的坐标
%  L2    由遗传算法得出的最短路径长度
%  XY2   由遗传算法得出的最短路径经过节点的坐标
%程序输出的图片有
%  Fig1  环境地图(包括:边界、障碍物、障碍物顶点之间的连线、Dijkstra的网络图结构)
%  Fig2  由Dijkstra算法得到的最短路径
%  Fig3  由遗传算法得到的最短路径
%  Fig4  遗传算法的收敛曲线(迄今为止找到的最优解、种群平均适应值)
%% 画Fig1
figure(1);
PlotGraph;
title('地形图及网络拓扑结构')
PD=inf*ones(26,26);
for i=1:26
    for j=1:26
        if D(i,j)==1
            x1=XY(i,5);
            y1=XY(i,6);
            x2=XY(j,5);
            y2=XY(j,6);
            dist=((x1-x2)^2+(y1-y2)^2)^0.5;
            PD(i,j)=dist;
        end
    end
end
%% 调用最短路算法求最短路
s=1;%出发点
t=26;%目标点
[L,R]=ZuiDuanLu(PD,s,t);
L1=L(end);
XY1=XY(R,5:6);
%% 绘制由最短路算法得到的最短路径
figure(2);
PlotGraph;
hold on
for i=1:(length(R)-1)
    x1=XY1(i,1);
    y1=XY1(i,2);
    x2=XY1(i+1,1);
    y2=XY1(i+1,2);
    plot([x1,x2],[y1,y2],'k');
    hold on
end
title('由Dijkstra算法得到的初始路径')
%% 使用遗传算法进一步寻找最短路
%第一步:变量初始化
M=50;%进化代数设置
N=20;%种群规模设置
Pm=0.3;%变异概率设置
LC1=zeros(1,M);
LC2=zeros(1,M);
Yp=L1;
%第二步:随机产生初始种群
X1=XY(R,1);
Y1=XY(R,2);
X2=XY(R,3);
Y2=XY(R,4);
for i=1:N
    farm{i}=rand(1,aaa);
end
% 以下是进化迭代过程
counter=0;%设置迭代计数器
while counter<M%停止条件为达到最大迭代次数    
    %% 第三步:交叉
    %交叉采用双亲双子单点交叉
    newfarm=cell(1,2*N);%用于存储子代的细胞结构
    Ser=randperm(N);%两两随机配对的配对表
    A=farm{Ser(1)};%取出父代A
    B=farm{Ser(2)};%取出父代B
    P0=unidrnd(aaa-1);%随机选择交叉点
    a=[A(:,1:P0),B(:,(P0+1):end)];%产生子代a
    b=[B(:,1:P0),A(:,(P0+1):end)];%产生子代b
    newfarm{2*N-1}=a;%加入子代种群
    newfarm{2*N}=b;
    for i=1:(N-1)
        A=farm{Ser(i)};
        B=farm{Ser(i+1)};
        newfarm{2*i}=b;
    end
    FARM=[farm,newfarm];%新旧种群合并    
    %% 第四步:选择复制
    SER=randperm(2*N);
    FITNESS=zeros(1,2*N);
    fitness=zeros(1,N);
    for i=1:(2*N)
        PP=FARM{i};
        FITNESS(i)=MinFun(PP,X1,X2,Y1,Y2);%调用目标函数
    end
    for i=1:N
        f1=FITNESS(SER(2*i-1));
        f2=FITNESS(SER(2*i));
        if f1<=f2
        else
            farm{i}=FARM{SER(2*i)};
            fitness(i)=FITNESS(SER(2*i));
        end
    end    
    %记录最佳个体和收敛曲线
    minfitness=min(fitness);
    meanfitness=mean(fitness);
    if minfitness<Yp
        pos=find(fitness==minfitness);
        Xp=farm{pos(1)};
        Yp=minfitness;
    end
    if counter==10
        PPP=[0.5,Xp,0.5]';
        PPPP=1-PPP;
        X=PPP.*X1+PPPP.*X2;
        Y=PPP.*Y1+PPPP.*Y2;
        XY2=[X,Y];
        figure(3)
        PlotGraph;
        hold on
        for i=1:(length(R)-1)
            x1=XY2(i,1);
            y1=XY2(i,2);
            x2=XY2(i+1,1);
            y2=XY2(i+1,2);
            plot([x1,x2],[y1,y2],'k');
            hold on
        end
        title('遗传算法第10代')
        hold on
        for i=1:(length(R)-1)
            x1=XY1(i,1);
            y1=XY1(i,2);
            x2=XY1(i+1,1);
            y2=XY1(i+1,2);
            plot([x1,x2],[y1,y2],'k','LineWidth',1);
            hold on
        end
    end    
    if counter==20
        PPP=[0.5,Xp,0.5]';
        PPPP=1-PPP;
        X=PPP.*X1+PPPP.*X2;
        Y=PPP.*Y1+PPPP.*Y2;
        XY2=[X,Y];
        figure(4)
        PlotGraph;
        hold on
        for i=1:(length(R)-1)
            x1=XY2(i,1);
            y2=XY2(i+1,2);
            plot([x1,x2],[y1,y2],'k');
            hold on
        end
        title('遗传算法第20代')
        hold on
        for i=1:(length(R)-1)
            x1=XY1(i,1);
            y1=XY1(i,2);
            x2=XY1(i+1,1);
            y2=XY1(i+1,2);
            plot([x1,x2],[y1,y2],'k','LineWidth',1);
            hold on
        end
    end
    if counter==30
        PPP=[0.5,Xp,0.5]';
        PPPP=1-PPP;
        X=PPP.*X1+PPPP.*X2;
        Y=PPP.*Y1+PPPP.*Y2;
        XY2=[X,Y];
        figure(5)
        PlotGraph;
        hold on
        for i=1:(length(R)-1)
            x1=XY2(i,1);
            y1=XY2(i,2);
            x2=XY2(i+1,1);
            y2=XY2(i+1,2);
            plot([x1,x2],[y1,y2],'k');
            hold on
        end
        title('遗传算法第30代')
        hold on
        for i=1:(length(R)-1)
            x1=XY1(i,1);
            y2=XY1(i+1,2);
            plot([x1,x2],[y1,y2],'k','LineWidth',1);
            hold on
        end
    end
    if counter==40
        PPP=[0.5,Xp,0.5]';
        PPPP=1-PPP;
        X=PPP.*X1+PPPP.*X2;
        Y=PPP.*Y1+PPPP.*Y2;
        XY2=[X,Y];
        figure(6)
        PlotGraph;
        hold on
        for i=1:(length(R)-1)
            x1=XY2(i,1);
            y1=XY2(i,2);
            x2=XY2(i+1,1);
            y2=XY2(i+1,2);
            plot([x1,x2],[y1,y2],'k');
            hold on
        end
        title('遗传算法第40代')
        hold on
        for i=1:(length(R)-1)
            x1=XY1(i,1);
            y1=XY1(i,2);
            x2=XY1(i+1,1);
            y2=XY1(i+1,2);
            plot([x1,x2],[y1,y2],'k','LineWidth',1);
            hold on
        end
    end
    if counter==50
        PPP=[0.5,Xp,0.5]';
        PPPP=1-PPP;
        X=PPP.*X1+PPPP.*X2;
        Y=PPP.*Y1+PPPP.*Y2;
        XY2=[X,Y];
        figure(7)
        PlotGraph;
        hold on
        for i=1:(length(R)-1)
            x1=XY2(i,1);
            y1=XY2(i,2);
            x2=XY2(i+1,1);
            y2=XY2(i+1,2);
            plot([x1,x2],[y1,y2],'k');
            hold on
        end
        title('遗传算法第50代')
        hold on
        for i=1:(length(R)-1)
            x1=XY1(i,1);
            y1=XY1(i,2);
            x2=XY1(i+1,1);
            y2=XY1(i+1,2);
            plot([x1,x2],[y1,y2],'k','LineWidth',1);
            hold on
        end
    end    
    LC2(counter+1)=Yp;
    LC1(counter+1)=meanfitness;    
    %% 第五步:变异
    for i=1:N
        if Pm>rand&&pos(1)~=i
            AA=farm{i};
            AA(POS)=rand;
            farm{i}=AA;
        end
    end    
    counter=counter+1;
    disp(counter);
end
%% 输出遗传算法的优化结果
PPP=[0.5,Xp,0.5]';
PPPP=1-PPP;
X=PPP.*X1+PPPP.*X2;
Y=PPP.*Y1+PPPP.*Y2;
XY2=[X,Y];
L2=Yp;
%% 绘制Fig3
figure(8)
PlotGraph;
hold on
hold on
for i=1:(length(R)-1)
    x1=XY1(i,1);
    y1=XY1(i,2);
    x2=XY1(i+1,1);
    y2=XY1(i+1,2);
    plot([x1,x2],[y1,y2],'k','LineWidth',1);
    hold on
end
for i=1:(length(R)-1)
    x1=XY2(i,1);
    y1=XY2(i,2);
    x2=XY2(i+1,1);
    y2=XY2(i+1,2);
    plot([x1,x2],[y1,y2],'k');
    hold on
end
title('遗传算法最终结果')
figure(9)
PlotGraph;
hold on
for i=1:(length(R)-1)
    x1=XY1(i,1);
    y1=XY1(i,2);
    x2=XY1(i+1,1);
    y2=XY1(i+1,2);
    plot([x1,x2],[y1,y2],'k','LineWidth',1);
    hold on
end
hold on
for i=1:(length(R)-1)
    x1=XY2(i,1);
    y1=XY2(i,2);
    x2=XY2(i+1,1);
    y2=XY2(i+1,2);
    plot([x1,x2],[y1,y2],'k','LineWidth',2);
    hold on
end
title('遗传算法优化前后结果比较')
%% 绘制Fig4
figure(10);
plot(LC1);
hold on
plot(LC2);
xlabel('迭代次数');
title('收敛曲线');

欢迎访问GreenSim团队主页:http://blog.sina.com.cn/greensim
[color=red]欢迎访问GreenSim——算法仿真团队→[url=http://blog.sina.com.cn/greensim]http://blog.sina.com.cn/greensim[/url][/color]
QQ:761222791