1 简介
本文提出一种基于蚁群算法和Maklink图的机器人路径规划方法,所述方法包括以下步骤:根据移动机器人工作环境建立相应的Maklink地图,采用多边形代表环境中的障碍物,通过自由链接线组成移动机器人自由移动的网络图;使用蚁群算法在Maklink地图上求取最优路径。
机器人路径规划的前提是环境建模,通过相机、雷达等传感器提取到的信息,分析计算得到机器人认识的环境地图,使机器人在该环境中进行路径规划. 目前常用的环境建模方法有可视图法 (visibilitygraph)、链接图法(MAKLINK graph)、栅格图法(grids)等.栅格图法虽然应用广泛,但存在复杂环境下环境信息储存量大、抗干扰能力弱、决策效率低下的缺点.链接图法具有占用内存小、搜索复杂性低的优点,因此本文采用链接图法建立地图.链接图的建立基于以下假设:
1) 多边形的高度平行于 Z 轴, 整个路径存在于XY 平面;
2)将障碍物的边界依据机器人的最大尺寸和机器人正常感知所需的最小范围进行扩展,将机器人简化为一个质点.障碍物用顶点表示,假设第i个障碍物Oi有ni个顶点,整个环境可表示为
机器人环境中的自由空间是由自由链接线围成的凸区域构建的,自由链接线满足4个条件:
1)链接线的两端必须是两个多边形障碍物的两个顶点,或者其中一个是障碍物顶点而另一个在环境的边界上,在此意义下同一障碍物顶点的连线也计算在内;
2)每条自由链接线是相邻两个自由凸区域的公共界线;
3)自由链接线不能穿越环境中的任何障碍物;
4)每个自由凸区域至少有两条自由链接线作为边界.将自由链接线的中点作为路径点,路径点顺序为1, 2, . . . , n, 路径点的连线为机器人可自由移动的网络.
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];
minlen = min(len);
minlen = minlen(1);
minant = find(len == minlen);
minant = minant(1);
% 更新全局最短路径
if minlen < LL
LL = minlen;
end
% 更新信息素
for i=1:pathCount
phePara(i,pathk(i,minant)) = (1-pheUpPara(1))* phePara(i,pathk(i,minant))+pheUpPara(1)*(1/minlen);
end
shortestpath(num) = minlen;
end
figure;
plot(1:NC,shortestpath,'color','blue');
hold on
% plot(1:NC,dijpathlen,'color','red');
ylabel('路径总长度');
xlabel('迭代次数');
3 仿真结果
4 参考文献
[1]秦国选, 郭伟, 王磊, 安蔚瑾, 孙日杰, & 陈辰. (2019). 基于蚁群算法和Maklink图的机器人路径规划方法. CN110045738A.
5 MATLAB代码与数据下载地址
见博客主页
