(转载)基于蚁群算法的二维路径规划(matlab实现)

1 理论基础

1.1 路径规划算法

        路径规划算法是指在有障碍物的工作环境中寻找一条从起点到终点的、无碰撞地绕过所有障碍物的运动路径。路径规划算法较多，大体上可分为全局路径规划算法和局部路径规划算法两类。其中，全局路径规划方法包括位形空间法、广义锥方法、顶点图像法、栅格划归法；

局部路径规划算法主要有人工势场法等。

1.2 MAKLINK图论理论

        MAKLINK图论可以建立二维路径规划的空间模型，MAKLINK图论通过生成大量的MAKLINK线构造二维路径规划可行空间，MAKLINK线定义为两个障碍物之间不与障碍物相交的顶点之间的连线，以及障碍物顶点与边界相交的连线。

        在MAKLINK图上存在n条自由连接线，连接线的中点依次为v1,v2,…,vn，连接所有MAKLINK线的中点加上始点S和终点T构成用于初始路径规划的无向网络图，如图2所示。

图2 无向网络图

1.3 蚁群算法

        蚁群算法是由Dorigo.M等人在20世纪90年代初提出的一种新型进化算法，它来源于对蚂蚁搜索问题的研究。人们在观察蚂蚁搜索食物时发现，蚂蚁在寻找食物时，总在走过的路径上释放一种称为信息素的分泌物，信息素能够保留一段时间，使得在一定范围内的其他蚂蚁能够觉察到该信息素的存在。后继蚂蚁在选择路径时，会选择信息素浓度较高的路径，并且在经过时留下自己的信息素，这样该路径的信息素会不断增强，蚂蚁选择的概率也在不断增大。蚁群算法最优路径寻找如图3所示。

图3 蚂蚊觅食过程

        图3表达了蚂蚁在觅食过程中的三个过程，其中点A是蚂蚁蚁巢，点D是食物所在地，四边形EBFCE表示在蚁巢和食物之间的障碍物。蚂蚁如果想从蚁巢点A达到点D,只能经过路径BFC或者路径BEC,假定从蚁巢中出来若干只蚂蚁去食物所在地D搬运食物，每只蚂蚁经过后留下的信息素为1,信息素保留的时间为1。一开始，路径BFC和BEC上都没有信息素，在点A的蚂蚁可以随机选择路径，蚂蚁以相同的概率选择路径BFC或BEC,如图3(b)所示。由于BFC路径长度只是BEC路径长度的一半，所以在一段时间内经过BFC到达点D的蚂蚁数量是经过BEC到达点D数量的两倍，在路径BFC上积累的信息素的浓度就是在路径BEC上积累的信息素浓度的两倍。这样在蚂蚁选择路径的时候，选择路径BFC的概率大于选择路径BEC的概率，随着时间的推移，蚂蚁将以越来越大的概率选择路径BFC,最终会完全选择路径BFC作为从蚁巢出发到食物源的路径，如图3(c)所示。

1.4 dijkstra算法

        dijkstra算法是典型的单源最短路径算法，用于计算非负权值图中一个节点到其他所有节点的最短路径，其基本思想是把带权图中所有节点分为两组，第1组包括已确定最短路径的节点，第2组为未确定最短路径的节点。按最短路径长度递增的顺序逐个把第2组的节点加入第1组中，直到从源点出发可到达的所有节点都包含在第1组中。

        dijkstra算法流程如下：

        (1)初始化存放未确定最短路径的节点集合V和已确定最短路径的节点集合S,利用带权图的邻接矩阵arcs初始化原点到其他节点最短路径长度D,如果源点到其他节点有连接弧，对应的值为连接弧的权值，否则对应的值取为极大值。

        (2)选择D中的最小值D[i],D[i]是源点到点i的最短路径长度，把点i从集合V中取出并放入集合S中。

        (3)根据节点i修改更新数组D中源点到集合V中的节点k对应的路径长度值。

        (4)重复步骤(2)与步骤(3)的操作，直至找出源点到所有节点的最短路径为止。

2 案例背景

2.1 问题描述

        采用蚁群算法在200×200的二维空间中寻找一条从起点S到终点T的最优路径，该二维空间中存在4个障碍物，障碍物1的4个顶点的坐标分别为(40 140;60 160;100 140;60 120),障碍物2的4个顶点分为别(50 30; 30 40;80 80;100 40),障碍物3的4个顶点分别为(120 160;140 100;180 170;165 180),障碍物4的3个顶点分别为(120 40;170 40;140 80),其中点S为起点，起点坐标为(20,180);点T为终点，终点坐标为(160,90)。二维规划空间如图4所示。

 2.2 算法流程

        算法流程如图5所示。其中，空间模型建立利用MAKLINK图论算法建立路径规划的二维空间，初始路径规划利用dijkstra算法规划出一条从起点到终点的初始路径，初始化算法参数，信息素更新采用根据蚂蚁搜索到的路径的长短优劣更新节点的信息素。

2.3 蚁群算法实现

        1.解的表示

        利用dijkstra算法在MAKLINK图上产生依次通过路径节点S,P1,P2,…,Pd,T的一条次最优路径。节点对应的自由链接线依次为Li(i=1,2,…,d)。设P(0)i和P(1)i为Li的两个端点，链路上的其他点表示方法为

        通过dijkstra算法得到路径经过的自由链接线时，只要给定一组参数(h1,h2,…,hd),就可以得到一条从起点到终点的新路径，蚁群算法的解即表示为(h1,h2,…,hd)。

        采用蚁群算法时需要离散化工作空间，由于初始化选择的自由链接线长短不一，对链接线的划分采用固定距离划分法，设定划分长度为ζ ,每条自由链接线L的划分数为

        2.节点选择

        蚁群算法优化寻找路径参数集合(h1,h2,…,hd),使得在离散化的空间里得到最短的路径。假设共有m只蚂蚁从起点S出发到达终点T,循环路径为S→n1j→n2j→…→ndj→T,其中，ndj表示路径点在第d条链接线的第j个等分点上。在移动过程中，当蚂蚁在链接线Li上时，选择下一个链接线Li+1上节点j的方法为

        3.信息素更新

        信息素更新包括实时信息素更新和路径信息素更新，其中实时信息素更新是指每一只蚂蚁在选择某个节点后都必须对该节点的信息素进行更新，即

        当所有蚂蚁从初始点走到终点，完成依次迭代搜索时，选择所有蚂蚁经过路径中长度最短的一条，更新该条路径上每一个点的信息素，即

3 MATLAB程序

        根据蚁群算法原理，在MATLAB软件中编程实现基于蚁群算法的二维路径规划算法，算法分为两步：第一步使用dijkstra算法生成初始次优路径；第二步在初始路径的基础上，使用蚁群算法生成全局最优路径。

3.1 dijkstra算法

        采用dijkstra算法规划初始路径，其算法思想是先计算点点之间的距离，然后依次计算各点到出发点的最短距离，程序如下：

function path = DijkstraPlan(position,sign)
%% 基于Dijkstra算法的路径规划算法
%position    input     %节点位置
%sign        input     %节点间是否可达
 
%path        output    %规划路径
 
%% 计算路径距离
cost = ones(size(sign))*10000;
[n,m] = size(sign);
for i = 1:n
    for j = 1:m
        if sign(i,j) == 1
            cost(i,j) = sqrt(sum((position(i,:)-position(j,:)).^2));
        end
    end
end
 
%% 路径开始点
dist = cost(1,:);             %节点间路径长度           
s = zeros(size(dist));        %节点经过标志
s(1) = 1;dist(1) = 0;
path = zeros(size(dist));     %依次经过的节点
path(1,:) = 1;
 
%% 循环寻找路径点
for num = 2:n   
    
    % 选择路径长度最小点
    mindist = 10000;
    for i = 1:length(dist)
        if s(i) == 0
            if dist(i)< mindist
                mindist = dist(i);
                u = i;
            end
        end
    end
    
    % 更新点点间路径
    s(u) = 1;
    for w = 1:length(dist)
        if s(i) == 0
            if dist(u)+cost(u,w) < dist(w)
                dist(w) = dist(u)+cost(u,w);
                path(w) = u;
            end
        end
    end
end

3.2 蚁群算法

        在初始路径的基础上，采用蚁群算法搜索最优路径，程序如下：

%% 清空环境
clc;clear

%% 障碍物数据
position = load('barrier.txt');
plot([0,200],[0,200],'.');
hold on
B = load('barrier.txt');
xlabel('km','fontsize',12)
ylabel('km','fontsize',12)
title('二维规划空间','fontsize',12)
%% 描述起点和终点
S = [20,180];
T = [160,90];
plot([S(1),T(1)],[S(2),T(2)],'.');

% 图形标注
text(S(1)+2,S(2),'S');
text(T(1)+2,T(2),'T');
 
%% 描绘障碍物图形
fill(position(1:4,1),position(1:4,2),[0,0,0]);
fill(position(5:8,1),position(5:8,2),[0,0,0]);
fill(position(9:12,1),position(9:12,2),[0,0,0]);
fill(position(13:15,1),position(13:15,2),[0,0,0]);

% 下载链路端点数据
L = load('lines.txt');
 
%% 描绘线及中点
v = zeros(size(L));
for i=1:20
    plot([position(L(i,1),1),position(L(i,2),1)],[position(L(i,1),2)...
        ,position(L(i,2),2)],'color','black','LineStyle','--');
    v(i,:) = (position(L(i,1),:)+position(L(i,2),:))/2;
    plot(v(i,1),v(i,2),'*');
    text(v(i,1)+2,v(i,2),strcat('v',num2str(i)));
end
 
%% 描绘可行路径
sign = load('matrix.txt');
[n,m]=size(sign);
 
for i=1:n
    
    if i == 1
        for k=1:m-1
            if sign(i,k) == 1
                plot([S(1),v(k-1,1)],[S(2),v(k-1,2)],'color',...
                    'black','Linewidth',2,'LineStyle','-');
            end
        end
        continue;
    end
    
    for j=2:i
        if i == m
            if sign(i,j) == 1
                plot([T(1),v(j-1,1)],[T(2),v(j-1,2)],'color',...
                    'black','Linewidth',2,'LineStyle','-');
            end
        else
            if sign(i,j) == 1
                plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],...
                    'color','black','Linewidth',2,'LineStyle','-');
            end
        end
    end
end
path = DijkstraPlan(position,sign);
j = path(22);
plot([T(1),v(j-1,1)],[T(2),v(j-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
i = path(22);
j = path(i);
count = 0;
while true
    plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
    count = count + 1;
    i = j;
    j = path(i);
    if i == 1 || j==1
        break;
    end
end
plot([S(1),v(i-1,1)],[S(2),v(i-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');


count = count+3;
pathtemp(count) = 22;
j = 22;
for i=2:count
    pathtemp(count-i+1) = path(j);
    j = path(j);
end
path = pathtemp;
path = [1     9     8     7    13    14    12    22];

%% 蚁群算法参数初始化
pathCount = length(path)-2;          %经过线段数量
pheCacuPara=2;                       %信息素计算参数
pheThres = 0.8;                      %信息素选择阈值
pheUpPara=[0.1 0.0003];              %信息素更新参数
qfz= zeros(pathCount,10);            %启发值

phePara = ones(pathCount,10)*pheUpPara(2);         %信息素
qfzPara1 = ones(10,1)*0.5;           %启发信息参数
qfzPara2 = 1.1;                      %启发信息参数
m=10;                                %种群数量
NC=500;                              %循环次数
pathk = zeros(pathCount,m);          %搜索结果记录
shortestpath = zeros(1,NC);          %进化过程记录
 
%% 初始最短路径
dijpathlen = 0;
vv = zeros(22,2);
vv(1,:) = S;
vv(22,:) = T;
vv(2:21,:) = v;
for i=1:pathCount-1
dijpathlen = dijpathlen + sqrt((vv(path(i),1)-vv(path(i+1),1))^2+(vv(path(i),2)-vv(path(i+1),2))^2);
end
LL = dijpathlen;
 
%% 经过的链接线
lines = zeros(pathCount,4);
for i = 1:pathCount
    lines(i,1:2) = B(L(path(i+1)-1,1),:);
    lines(i,3:4) = B(L(path(i+1)-1,2),:);
end
 
%% 循环搜索
for num = 1:NC
    
    %% 蚂蚁迭代寻优一次
    for i=1:pathCount
        for k=1:m
            q = rand();
            qfz(i,:) = (qfzPara2-abs((1:10)'/10-qfzPara1))/qfzPara2; %启发信息
            if q<=pheThres%选择信息素最大值
                arg = phePara(i,:).*(qfz(i,:).^pheCacuPara);
                j = find(arg == max(arg));
                pathk(i,k) = j(1);
            else  % 轮盘赌选择
                arg = phePara(i,:).*(qfz(i,:).^pheCacuPara);
                sumarg = sum(arg);
                qq = (q-pheThres)/(1-pheThres);
                qtemp = 0;
                j = 1;
                while qtemp < qq
                    qtemp = qtemp + (phePara(i,j)*(qfz(i,j)^pheCacuPara))/sumarg;
                    j=j+1;
                end
                j=j-1;
                pathk(i,k) = j(1);
            end
            % 信息素更新
            phePara(i,j) = (1-pheUpPara(1))*phePara(i,j)+pheUpPara(1)*pheUpPara(2);
        end
    end
    
    %% 计算路径长度
    len = zeros(1,k);
    for k=1:m
        Pstart = S;
        Pend = lines(1,1:2) + (lines(1,3:4)-lines(1,1:2))*pathk(1,k)/10;
        for l=1:pathCount
            len(1,k) = len(1,k)+sqrt(sum((Pend-Pstart).^2));
            Pstart = Pend;
            if l

4 结果分析

        在无向网络图的基础上采用dijkstra算法规划初始路径，初始路径规划结果如图6中粗实线所示。

        在初始路径规划的基础上采用蚁群算法进行详细路径规划，首先根据初始路径规划结果判断路径经过的链路为v6→v7→v8→v11→v12→v13,每条链路均离散化为10个小路段，种群个体数为10,个体长度为6,算法进化次数共500次，迭代过程中适应度变化以及规划出的路径如图7与图8所示，其中图8中虚线为蚁群算法规划出的最优路径

5 延伸阅读

5.1 蚁群算法改进

        当蚂蚁在节点pi-1上搜索下一个节点pi时，基本蚁群算法是根据信息素和距离计算从pi-1点

到下一条链路上所有节点的概率，然后从中选择下一个节点pi。由于每次节点的选择都是在减少

从当前节点到终点的总长度，因此如果选择节点pi-1和pi的夹角同起点和终点的夹角一致或者相

差不大，则pi应该是优先考虑的点，如图9所示。其中，pa和pa为链路的两个端点；点S为路径规划起点；点T为路径规划终点；p-1为当前蚂蚁所在点；p;为蚂蚁下一个搜索点；α为节点连线和起点终点连线间的夹角。

6 代码获取

基于蚁群算法的旅行商问题(TSP)求解(matlab实现)资源-CSDN文库