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 all;
close all;
tic
%% 障碍物数据
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)
%% 描述起点和终点
Start = [20,180];
Target = [160,90];
plot([Start(1),Target(1)],[Start(2),Target(2)],'.');
% 图形标注
text(Start(1),Start(2)+7,'S');
text(Target(1)+3,Target(2)-4,'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');
plot([Start(1),v(i-1,1)],[Start(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; %到此能在规划空间中用黄线描绘出次优最短路径
pathCount = length(path)-2; %经过线段数量，相当于维数D
%% 经过的链接线
lines = zeros(pathCount,4);
for i = 1:pathCount %lines用于存放各个结点所在链接线的起点和终点
lines(i,1:2) = B(L(path(i+1)-1,1),:);
lines(i,3:4) = B(L(path(i+1)-1,2),:);
end
%% 初始最短路径
dijpathlen = 0;
vv = zeros(22,2);
vv(1,:) = Start;
vv(22,:) = Target;
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; %得出的LL为次优最短路径的长度
%% 果蝇优化算法参数初始化
popsize=pathCount;
maxgen=200;
gen=0;
D=zeros(pathCount,pathCount);
S=zeros(pathCount,pathCount);
bestgensmell=zeros(1,maxgen);
%% 果蝇位置初始化及食物浓度计算
%*** 随机初始果蝇群体位置。
X_axis=1+1*rands(1,pathCount);
Y_axis=1+1*rands(1,pathCount);
%*** 果蝇寻优开始，利用嗅觉寻找食物。
for p=1:popsize
X(p,:)=X_axis+2*rand()-1;
Y(p,:)=Y_axis+2*rand()-1;
for i=1:pathCount
%*** 求出与原点之距离
D(p,i)=(X(p,i)^2+Y(p,i)^2)^0.5;
%*** 味道浓度为距离之倒数，先求出味道浓度判定值。
S(p,i)=1/D(p,i);
end
end
%*** 利用味道浓度判定函数求出味道浓度
for p=1:pathCount
w=S(:,p);
Smell(p)=distance(w,pathCount,lines);
end
%% 初始化公告板
[bestsmell bestindex]=min(Smell);
%*** 利用视觉寻找伙伴聚集味道浓度最高之处，做法是保留最佳值初始位置及初始味道浓度。
X_axis=X(bestindex,:);
Y_axis=Y(bestindex,:);
bestS=S(bestindex,:);
SmellBest=bestsmell;
%画出公告板中的最短路径所对应的人工鱼的轨迹
lastline=zeros(pathCount+2,2);
lastline(1,:)=Start;
lastline(pathCount+2,:)=Target;
pathkbest=bestS;
for i=1:pathCount
lastline(i+1,:)=lines(i,1:2) + (lines(i,3:4)-lines(i,1:2))*pathkbest(:,i);
end
for i=1:pathCount+1
plot([lastline(i,1),lastline(i+1,1)],[lastline(i,2),lastline(i+1,2)],'color','red','LineWidth',3);
end
%画出最路径随迭代次数的变化
figure(2);
% plot(bestgensmell,'color','red');
plot(bestgensmell);
hold on
title( ['本次运行得到的最优值为',num2str(SmellBest),',总共耗时',num2str(t_train),'s'] );
%text(180,190,'最优值随迭代次数变化曲线');
ylabel('路径总长度');
xlabel('迭代次数');
% figure(3);
% plot(Xbest,Ybest,'b.');
% title('Fruit fly flying route','fontsize',14)
% xlabel('X-axis','fontsize',12);ylabel('Y-axis','fontsize',12);
3 仿真结果
4 参考文献
[1]柳琦. 基于改进果蝇优化算法的机器人路径规划问题研究. Diss. 大连海事大学.
部分理论引用网络文献，若有侵权联系博主删除。
5 MATLAB代码与数据下载地址
见博客主页
