1 简介
通过栅格法建立栅格地图作为机器人路径规划的工作环境,采用蚁群算法作为机器人路径搜索的规则.将所有机器人放置于初始位置.经过NC次无碰撞迭代运动找到最优路径.到达目标位置.为防止机器人在路径搜索过程中没有达到最大迭代次数时路径大小已不发生变化而陷入局部最优.可通过对各路径上的信息素进行增减来使机器人路径搜索跳出当前值继续搜索.直到迭代完毕,获得最优路径.
2 部分代码
clc
clear all
close all
source= strcat('ESACCI-SEAICE-L4-SICONC-AMSR_25.0kmEASE2-NH-20151202-fv2.0.nc');
Lon_Data = double(abs(ncread(source,'lon')));
Lat_Data = double(ncread(source,'lat'));
num_shange=30;%栅格的数量
num_point=num_shange*num_shange;%一共900个格子，按从左到右，从下到上的顺序对格子编号
sign=zeros(num_shange,num_shange);%障碍矩阵信息
ice=zeros(num_shange,num_shange);%障碍矩阵信息
% for i=1:size(Lon_Data,1)
% for j=1:size(Lon_Data,1)
% for m=1:num_shange
% t1=16+m*0.5;
% for n=1:num_shange
% t2=135+n*0.5;
% if Lat_Data(i,j)<t1&&Lat_Data(i,j)>t1-0.5&&Lon_Data(i,j)<t2&&Lon_Data(i,j)>t2-0.5
% ice(m,n)= ice(m,n)+1;
% end
% end
% end
%
% end
% end
% ice=ice+ice';
load data4.mat
zhangai_point=[];
for i=1:num_shange
for j=1:num_shange
if ice(i,j)>6
sign1(i,j)=1;%设置障碍
zhangai_point=[zhangai_point i*num_shange+j];
end
end
end
G=sign;
MM=size(G,1); % G 地形图为01矩阵，如果为1表示障碍物
Tau=ones(MM*MM,MM*MM); % Tau 初始信息素矩阵
Tau=8.*Tau;
K=100; %迭代次数（指蚂蚁出动多少波）
M=50; %蚂蚁个数
S=1 ; %最短路径的起始点
E=MM*MM; %最短路径的目的点
Alpha=1; % Alpha 表征信息素重要程度的参数
Beta=7; % Beta 表征启发式因子重要程度的参数
Rho=0.3 ; % Rho 信息素蒸发系数
Q=1; % Q 信息素增加强度系数
minkl=inf; %minkl表示当前最短路径长度
mink=0; %当前完成最短路径为第几次迭代
minl=0; %当前完成最短路径为第只蚂蚁
D=G2D(G); %每个栅格至各自邻域无障碍栅格的代价值
N=size(D,1); %N表示问题的规模（象素个数）
a=1; %小方格象素的边长
Ex=a*(mod(E,MM)-0.5); %目的点横坐标
if Ex==-0.5
Ex=MM-0.5;
end
Ey=a*(MM+0.5-ceil(E/MM)); %目的点纵坐标
Eta=zeros(N); %启发式信息矩阵，记录启发式信息
a1=0.5;%路径长度系数
b1=0.5;%燃油系数
%以下开始建立启发式信息矩阵
); %选出本次迭代适应度值存储至minPL的相应迭代位置
end
%绘制“收敛曲线变化趋势”图，将每次迭代的最短路径放在图中表示
figure(2)
plot(minPL);
hold on
grid on
title('收敛曲线变化趋势');
xlabel('迭代次数');
ylabel('适应度值');
%绘爬行图
figure(3)
axis([0,MM,0,MM]) %设置图的横纵坐标，MM为地图矩阵的行数或列数
for i=1:MM
for j=1:MM
if G(i,j)==1 %1是黑色代表障碍，0为白色无障碍
x1=j-1;y1=MM-i;
x2=j;y2=MM-i;
x3=j;y3=MM-i+1;
x4=j-1;y4=MM-i+1;
fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]); %将1234点所围成的图形进行黑色填充
hold on
else
x1=j-1;y1=MM-i;
x2=j;y2=MM-i;
x3=j;y3=MM-i+1;
x4=j-1;y4=MM-i+1;
fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]); %将1234点所围成的图形进行白色填充
hold on
set(gca,'YTickLabel',[60:1:90]); %使得地图的矩阵的行列与正常坐标轴的行列一致
set(gca,'XTickLabel',[105:1:145]); %使得地图的矩阵的行列与正常坐标轴的行列一致
end
end
end
hold on
title('机器人路径轨迹');
xlabel('坐标x');
ylabel('坐标y');
ROUT=ROUTES{mink,minl};
%ROUT取最短行走路线，mink为该路线的迭代号,minl为该路线的蚂蚁号
LENROUT=length(ROUT);
%Rx与Ry中分别存储具体的该路线
Rx=ROUT;
Ry=ROUT;
%将该路线的栅格索引号转换为横纵坐标
for ii=1:LENROUT
Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);
if Rx(ii)==-0.5
Rx(ii)=MM-0.5;
end
Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));
end
plot(Rx,Ry) %绘各代蚂蚁爬行图
end
3 仿真结果
4 参考文献
[1]周东健, 张兴国, 马海波,等. 基于栅格地图-蚁群算法的机器人最优路径规划[J]. 南通大学学报:自然科学版, 2013, 12(4):91-94.
部分理论引用网络文献，若有侵权联系博主删除。
5 MATLAB代码与数据下载地址
见博客主页
