0
点赞
收藏
分享

微信扫一扫

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码

1 简介

一、栅格地图介绍

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

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_迭代【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_02

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_03【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_04

室内环境栅格法建模步骤

1.栅格粒大小的选取

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

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

2.障碍物栅格确定

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

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

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

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

二.蚁群算法基本原理

2.1 算法综述

  对于VRP问题,求解算法大致可分为精确算法和人工智能算法两大类。精确性算法基于严格的数学手段,在可以求解的情况下,解的质量较好。但是由于算法严格,运算量大,特别是大规模的问题几乎无法求解。所以其应用只能是小规模的确定性问题,面对中小规模问题,人工智能算法在精度上不占优势。但规模变大时,人工智能方法基本能在可接受时间里,找到可接受的满意解,这是精确算法难以做到的。由于的实际问题,各种约束错综复杂,人工智能算法显示出了巨大的优越性,也正因为如此,实际应用中,人工智能算法要更广泛。求解车辆路径调度问题的精确算法有动态规划法、分枝定界法等。并开始寻求所得结果可接受的启发式算法,以处理大规模实际问题,一些其他学科的新一代优化算法相继出现,如禁忌搜索算法,遗传算法,人工神经网络算法,以及现在研究较多的蚁群算法等。

2.2 蚁群算法的原理

  蚁群算法是受到对真实蚂蚁群觅食行为研究的启发而提出。生物学研究表明:一群相互协作的蚂蚁能够找到食物和巢穴之间的最短路径,而单只蚂蚁则不能。生物学家经过大量细致观察研究发现,蚂蚁个体之间的行为是相互作用相互影响的。蚂蚁在运动过程中,能够在它所经过的路径上留下一种称之为信息素的物质,而此物质恰恰是蚂蚁个体之间信息传递交流的载体。蚂蚁在运动时能够感知这种物质,并且习惯于追踪此物质爬行,当然爬行过程中还会释放信息素。一条路上的信息素踪迹越浓,其它蚂蚁将以越高的概率跟随爬行此路径,从而该路径上的信息素踪迹会被加强,因此,由大量蚂蚁组成的蚁群的集体行为便表现出一种信息正反馈现象。某一路径上走过的蚂蚁越多,则后来者选择该路径的可能性就越大。蚂蚁个体之间就是通过这种间接的通信机制实现协同搜索最短路径的目标的。我们举例简单说明蚂蚁觅食行为:

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_05【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_迭代_06

如上图a,b,c的示意图:

a图是原始状态,蚂蚁起始点为A,要到达E,中途有障碍物,要绕过才能到达。BC和BH是绕过障碍物的2条路径(假设只有2条)。各个路径的距离d已经标定。

b图是t=0时刻蚂蚁状态,各个边上有相等的信息素浓度,假设为15;

c图是t=1时刻蚂蚁经过后的状态,各个边的信息素浓度,有变化;因为大量蚂蚁的选择概率会不一样,而选择概率是和路径长度相关的。所以越短路径的浓度会越来越大,经过此短路径达到目的地的蚂蚁也会比其他路径多。这样大量的蚂蚁实践之后就找到了最短路径。所以这个过程本质可以概括为以下几点:

1.路径概率选择机制信息素踪迹越浓的路径,被选中的概率越大

2.信息素更新机制路径越短,路径上的信息素踪迹增长得越快

3.协同工作机制蚂蚁个体通过信息素进行信息交流。

从蚂蚁觅食的原理可见,单个个体的行为非常简单蚂蚁只知道跟踪信息素爬行并释放信息素,但组合后的群体智能又非常高蚂蚁群能在复杂的地理分布的清况下,轻松找到蚁穴与食物源之间的最短路径。这种特点恰恰与元启发算法的特点相一致,蚁群优化算法正是受到这种生态学现象的启发后加以模仿并改进而来,觅食的蚂蚁由人工蚁替代,蚂蚁释放的信息素变成了人工信息素,蚂蚁爬行和信息素的蒸发不再是连续不断的,而是在离散的时空中进行。

  上述例子如果不好理解,我在这里贴几张PPT,个人感觉非常不错,也是我找了很多资料觉得最好理解的【来源是大连理工大学谷俊峰】,点击这里提供下载:​​蚁群算法基本知识.rar​​。

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_07【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_08

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_09【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_10

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_11【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_迭代_12

从深层意义上来讲,蚁群算法作为优化的方法之一,属于人工群集智能领域。人工群集智能,大都受自然群集智能如昆虫群和动物群等的启发而来。除了具有独特的强有力的合作搜索能力外,还可以利用一系列的计算代理对问题进行分布式处理,从而大大提高搜索效率。

​​回到目录​​

三.蚁群算法的基本流程

  我们还是采用大连理工大学谷俊峰的PPT来说明问题,重要公式进行截图计算和解释,对PPT难以理解的地方进行单独解释:

3.1 基本数学模型

  首先看看基本TSP问题的基本数学模型:

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_迭代_13【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_14

  问题其实很简单,目标函数就是各个走过路径的总长度,注意的就是距离矩阵根据实际的问题不一样,长度是不一样的。

3.2 蚁群算法说明

  在说明群蚁算法流程之前,我们对算法原理和几个注意点进行描述:

1.TSP问题的人工蚁群算法中,假设m只蚂蚁在图的相邻节点间移动,从而协作异步地得到问题的解。每只蚂蚁的一步转移概率由图中的每条边上的两类参数决定:1. 信息素值也称信息素痕迹。2.可见度,即先验值。 2.信息素的更新方式有2种,一是挥发,也就是所有路径上的信息素以一定的比率进行减少,模拟自然蚁群的信息素随时间挥发的过程;二是增强,给评价值“好”(有蚂蚁走过)的边增加信息素。 3.蚂蚁向下一个目标的运动是通过一个随机原则来实现的,也就是运用当前所在节点存储的信息,计算出下一步可达节点的概率,并按此概率实现一步移动,逐此往复,越来越接近最优解。 4.蚂蚁在寻找过程中,或者找到一个解后,会评估该解或解的一部分的优化程度,并把评价信息保存在相关连接的信息素中。

3.3 蚁群算法核心步骤

  更加我们前面的原理和上述说明,群蚁算法的2个核心步骤是 路径构建 和 信息素更新。我们将重点对这2个步骤进行说明。

3.3.1 路径构建

  每个蚂蚁都随机选择一个城市作为其出发城市,并维护一个路径记忆向量,用来存放该蚂蚁依次经过的城市。蚂蚁在构建路径的每一步中,按照一个随机比例规则选 择下一个要到达的城市。随机概率是按照下列公式来进行计算的:

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_15【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_16

  上述公式就是计算 当前点 到 每一个可能的下一个节点 的概率。分子是 信息素强度 和 能见度 的幂乘积,而分母则是所有 分子的和值。这个刚开始是很不容易理解的,我们在最后实例计算的时候,可以看得很清楚,再反过来理解公式。注意每次选择好节点后,就要从可用节点中移除选择的节点。

3.3.2 信息素更新

  信息素更新是群蚁算法的核心。也是整个算法的核心所在。算法在初始期间有一个固定的浓度值,在每一次迭代完成之后,所有出去的蚂蚁回来后,会对所走过的路线进行计算,然后更新相应的边的信息素浓度。很明显,这个数值肯定是和蚂蚁所走的长度有关系的,经过一次次的迭代, 近距离的线路的浓度会很高,从而得到近似最优解。那我们看看信息素更新的过程。

  初始化信息素浓度C(0),如果太小,算法容易早熟,蚂蚁会很快集中到一条局部最优路径上来,因为可以想想,太小C值,使得和每次挥发和增强的值都差不多,那么 随机情况下,一些小概率的事件发生就会增加非最优路径的信息素浓度;如果C太大,信息素对搜索方向的指导性作用减低,影响算法性能。一般情况下,我们可以使用贪婪算法获取一个路径值Cnn,然后根据蚂蚁个数来计算C(0) = m/Cnn ,m为蚂蚁个数

  每一轮过后,问题空间中的所有路径上的信息素都会发生蒸发,然后,所有的蚂蚁根据自己构建的路径长度在它们本轮经过的边上释放信息素,公式如下:

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_17【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_18

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_19【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_蚁群算法_20

  信息素更新的作用: 1.信息素挥发(evaporation)信息素痕迹的挥发过程是每个连接上的 信息素痕迹的浓度自动逐渐减弱的过程,这个挥发过程主要用于避 免算法过快地向局部最优区域集中,有助于搜索区域的扩展。 2.信息素增强(reinforcement)增强过程是蚁群优化算法中可选的部 分,称为离线更新方式(还有在线更新方式)。这种方式可以实现 由单个蚂蚁无法实现的集中行动。基本蚁群算法的离线更新方式是 在蚁群中的m只蚂蚁全部完成n城市的访问后,统一对残留信息进行 更新处理。

3.3.3 迭代与停止

  迭代停止的条件可以选择合适的迭代次数后停止,输出最优路径,也可以看是否满足指定最优条件,找到满足的解后停止。最重要的是,我刚开始理解这个算法的时候,以为每一只蚂蚁走一条边就是一次迭代,其实是错的。这里算法每一次迭代的意义是:每次迭代的m只蚂蚁都完成了自己的路径过程,回到原点后的整个过程。

2 部分代码

clc;clear all;
L=[0 0 0 1 1 1 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 1 1 1 1 0 0 0 1 1 1 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 0 0 1 1 1 0 0 0 1 1 1 0 0 0 0 0;
0 1 1 1 0 0 0 0 1 0 0 0 1 1 1 0 0 0 0 0;
0 1 1 1 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0;
0 1 1 1 0 0 1 1 1 0 0 0 1 1 1 1 0 0 0 0 ;
0 1 1 1 0 0 0 0 1 0 0 0 1 1 1 1 0 0 0 0 ;
0 0 0 0 0 0 0 0 1 0 0 0 1 1 1 1 0 0 0 0 ;
0 0 0 0 0 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 1 1 1 1 0;
0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 ;
0 0 1 1 0 0 0 0 0 0 1 1 1 0 0 0 1 1 1 1 ;
0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 1 1 1 1 0;
0 0 0 0 0 0 1 1 1 0 1 1 0 0 0 0 0 1 1 0;
0 0 0 0 0 0 0 0 0 0 1 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 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;];
N=100;
M=60;
a=1;
b=6;
p=0.3 ;
Q=1;
q1=0.1;
% L 地形图为01矩阵,如果为0表示机器人能通过,如果为1表示障碍物,机器人不能通过
% N 迭代的次数
% M 蚁群中蚂蚁的个数
% a 表示信息启发式因子,反映了蚁群在运动过程中所残留的信息量的相对重要程度
% b 表示期望启发式因子,反映了期望值的相对重要程度
% p 表示信息素挥发系数
% Q 常量,表示蚂蚁循环一周或一个过程在经过的路径上所释放的信息素总量
%q1是(0,1)间的一个算法参量,在状态转移时作为一个判断基准
M1=size(L,1);
Set=1; % Set 为地图中路径起始点位置
En=M1*M1; % En为地图中终止点位置
H= route_distance (L); %通过调用子函数route_distance.m,产生地图中方格的邻接矩阵H用此矩
%阵存储蚂蚁可以先后通过的两个方格的位置及距离,这两个方格都是无障碍方格并且处于相
%邻或对角位置。H矩阵是蚂蚁机器人移动的根据。
K=size(H,1); %K表示问题的规模,也表示L地图上的方格总数
t1=ones(K,K); % t1 初始信息素矩阵,
t1=9.*t1; %将每条路径上的信息素数设为常数9;
d_t1=zeros(K,K);% d_t1 信息素增量矩阵
aa=1; %aa方格的边长
Enx=M1-0.5; %Enx 终止点的横坐标
Eny=0.5; %终止点纵坐标
y1=zeros(K); %启发式信息矩阵,表示蚂蚁在方格间转移的期望程度
%%下面构造启发式信息矩阵,每个节点的启发式信息素数量取为至目标点(在此为终止点)的
%直线距离的倒数
for i=1:K
ix=aa*(mod(i,M1)-0.5);
if ix==-0.5 %当i等于M1的整数倍时,ix=-0.5,即每一行的最后一个方格
ix=M1-0.5;
end
iy=aa*(M1+0.5-ceil(i/M1));
if i~=En
y1(i)=1/((ix-Enx)^2+(iy-Eny)^2)^0.5; %构造启发式信息矩阵
else
y1(i)=100;
end
end
Rs=cell(N,M); %用细胞结构存储每一代的每一只蚂蚁的爬行路线
Pr=zeros(N,M); %用矩阵存储每一代的每一只蚂蚁的爬行路线长度
kn=inf; k2=0; n2=0; %初始化过程参数
%N轮蚂蚁觅食开始,每轮派出的蚂蚁数为M
for k=1:N
for m=1:M
%% 第一步:初始化参数
S1=Set; %当前节点初始化为起始点
rt_1=Set; %rt_1存储当前蚂蚁的爬行路线
rd=0; %rd 爬行路线长度
tabuk=ones(K); %初始化禁忌表
tabuk(Set)=0; %排除起始点
H1=H; %邻接矩阵初始化
%% 第二步:搜寻蚂蚁下一步可以前往的方格
H2=H1(S1,:); % H2为H1矩阵的第一行,表示L矩阵第S1个元素(按行排列)与其他元素的
%连通关系
H3=find(H2); %返回H2中不为零元素的序号,按顺序排列
for j=1:length(H3)
if tabuk(H3(j))==0
H2(H3(j))=0;
end
end
L1=find(H2);
Len_L1=length(L1);%可选节点的个数
while S1~=En&&Len_L1>=1 % 觅食停止条件:蚂蚁到终点或者陷入死胡同
%% 第三步:确定蚂蚁下一步将要转移的位置
q=rand;
if q<=q1
t12=zeros(1,K);
for i=1:Len_L1
t12(i)= (t1(S1,L1(i))^a)*((y1(L1(i)))^b);
end
m_t12=max(t12);
select=find(t12>=m_t12);
to_visit=L1(select(1));
else
P1=zeros(Len_L1); %初始化状态转移概率矩阵
for i=1:Len_L1
P1(i)=(t1(S1,L1(i))^a)*((y1(L1(i)))^b);
end
sump1=sum(P1);
P1=P1/sump1;%计算状态转移概率
%%利用轮盘赌方式选择蚂蚁下一步要去的位置
P2(1)=P1(1);
for i=2:Len_L1
P2(i)=P2(i-1)+P1(i);
end
Select1=find(P2>=rand);
to_visit=L1 (Select1(1));
end
%% 第四步:更新状态并做有关记录
rt_1=[rt_1,to_visit];%路径增加
rd=rd+H1(S1,to_visit);%路径长度增加
S1=to_visit;%蚂蚁移到下一个节点位置
for k12=1:K
if tabuk(k12)==0
H1(S1,k12)=0;
H1(k12,S1)=0;
end
end
tabuk(S1)=0;%已访问过的节点从禁忌表中删除
H2=H1(S1,:);
H3=find(H2);
for j=1:length(H3)
if tabuk(H3(j))==0
H2(j)=0;
end
end
L1=find(H2);
Len_L1=length(L1);%可选节点的个数
end
%% 第五步:记录蚂蚁的爬行路线并且记录搜索到本轮最短路径的蚂蚁及路径长度
Rs{k,m}=rt_1; %存储第k轮第m个蚂蚁的路线
if rt_1(end)==En
Pr(k,m)=rd;
if rd<kn
k2=k;
n2=m;
kn=rd;
end
else
Pr(k,m)=0;
end
end
%% 第六步:执行全局更新
d_t1=zeros(K,K);
for m=1:M
if Pr(k,m)
Rt=Rs{k,m};
TS=length(Rt)-1;
PL_d=Pr(k,m);
for s=1:TS
x=Rt(s);
y=Rt(s+1);
d_t1(x,y)=d_t1(x,y)+Q/PL_d;
d_t1(y,x)=d_t1(y,x)+Q/PL_d;
end
end
end
t1=(1-p).*t1+d_t1;%信息素挥发一部分,新增加一部分
end
%%绘图
%绘制机器人寻优路径收敛曲线
minrt=zeros(N);
rt11=zeros(1,M);
for i=1:N
rt11=Pr(i,:);
Nzero=find(rt11);
rt12=rt11(Nzero);
minrt(i)=min(rt12);
end
figure(1)
plot(minrt);
hold on
grid on
title('机器人寻优路径收敛曲线');
xlabel('迭代的次数');
ylabel('蚂蚁搜寻路径长度');
%绘蚂蚁爬行图
figure(2)
axis([0,M1,0,M1])
for i=1:M1
for j=1:M1
if L(i,j)==1
x1=j-1;y1=M1-i; x2=j;y2=M1-i;
x3=j;y3=M1-i+1; x4=j-1;y4=M1-i+1;
fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);
hold on
else
x1=j-1;y1=M1-i; x2=j;y2=M1-i;
x3=j;y3=M1-i+1; x4=j-1;y4=M1-i+1;
fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
hold on
end
end
end
hold on
Rt=Rs{k2,n2};
LEN_Rt=length(Rt);
Rx=Rt;
Ry=Rt;
for ik=1:LEN_Rt
Rx(ik)=aa*(mod(Rt(ik),M1)-0.5);
if Rx(ik)==-0.5
Rx(ik)=M1-0.5;
end
Ry(ik)=aa*(M1+0.5-ceil(Rt(ik)/M1));
end
plot(Rx,Ry)

3 仿真结果

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_栅格_21

4 参考文献

[1]徐进. 基于蚁群算法的移动机器人路径规划算法研究[D]. 北京化工大学.

博主简介:擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,相关matlab代码问题可私信交流。

部分理论引用网络文献,若有侵权联系博主删除。

【机器人栅格地图】基于蚁群算法求解栅格地图路径规划及避障附matlab代码_迭代_22


举报

相关推荐

0 条评论