matlab基于蚁群算法的二维路径规划算法【matlab优化算法二十二】
- 格式：pdf
- 大小：341.81 KB
- 文档页数：8
matlab基于蚁群算法的⼆维路径规划算法【matlab优化算法⼆⼗⼆】
基于蚁群算法的⼆维路径规划算法
路径规划算法
路径规划算法是指在有障碍物的⼯作环境中寻找⼀条从起点到终点的、⽆碰撞地绕过所有障碍物的运动路径。路径规划算法较多，⼤体上可
分为全局路径规划算法和局部路径规划算法两类。其中，全局路径规划⽅法包括位形空间法、⼴义锥⽅法、顶点图像法、栅格划归法；局部
路径规划算法主要有⼈⼯势场法等。
MAKLINK图论理论
MAKLINK图论可以建⽴⼆维路径规划的空间模型， MAKLINK图论通过⽣成⼤量的MAKLINK线构造⼆维路径规划可⾏空间， MAKLINK线定义为两个障碍物之间不与障碍物相交的顶点之间的连线，以及障碍物顶点与边界相交的连线。典型 MAKLINE图形如图所⽰。
在 MAKLINK图上存在l条⾃由连接线，连接线的中点依次为v,v，…，v，连接所有MAKLINK线的中点加上始点S和终点T构成⽤于初始路径
规划的⽆向⽹络图
蚁群算法蚁群算法是由 dorigo.M等⼈在20世纪90年代初提出的⼀种新型进化算法，它来源于对蚂蚁搜索问题的研究。⼈们在观察蚂蚁搜索⾷物时
发现，蚂蚁在寻找⾷物时，总在⾛过的路径上释放⼀种称为信息素的分泌物，信息素能够保留⼀段时间，使得在⼀定范围内的其他蚂蚁能够
觉察到该信息素的存在。后继蚂蚁在选择路径时，会选择信息素浓度较⾼的路径，并且在经过时留下⾃⼰的信息素这样该路径的信息素会不断增强，蚂蚁选择的概率也在不断增⼤蚁群算法最优路径寻找如图。
图表达了蚂蚁在觅⾷过程中的三个过程，其中点A是蚂蚁蚁巢，点D是⾷物所在地，四边形 EBFCE表⽰在蚁巢和⾷物之间的障碍物。蚂蚁如
果想从蚁巢点A达到点D，只能经过路径BFC或者路径BEC，假定从蚁巢中出来若⼲只蚂蚁去⾷物所在地D搬运⾷物，每只蚂蚁经过后留下
的信息素为1，信息素保留的时间为1.⼀开始，路径BFC和BEC上都没有信息素，在点A的蚂蚁可以随机选择路径，蚂蚁以相同的概率选择
路径BFC或BEC，如图（b）所⽰。由于BFC路径长度只是BEC路径长度的⼀半，所以在⼀段时间内经过BFC到达点D的蚂蚁数量是经过
BEC到达点D数量的两倍，在路径BFC上积累的信息素的浓度就是在路径BEC上积累的信息素浓度的两倍。这样在蚂蚁选择路径的时候，选
择路径BFC的概率⼤于选择路径BEC的概率，随着时间的推移，蚂蚁将以越来越⼤的概率选择路径BFC，最终会完全选择路径BFC作为从蚁
巢出发到⾷物源的路径，如图（c）所⽰。
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）的操作，直⾄找出源点到所有节点的最短路径为⽌。
采⽤蚁群算法在200×200的⼆维空间中寻找⼀条从起点S到终点T的最优路径，该⼆维空间中存在4个障碍物，障碍物1的4个顶点的坐标
分别为（40140；60160；
100140；60120），障碍物2的4个顶点分为别（5030；3040；8080；10040），障碍物3的4个顶点分别为（120160；
140100；180170；165180），障碍物4的3个顶点分别为（12040；17040；14080），其中点S为起点，起点坐标为（20,180）；点T为终点，终点坐标为（160,90）。⼆维规划空间如图所⽰。
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
主程序
%% 清空环境
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)
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
