机器人路径跟踪 代码 博客_基于蚁群算法的机器人路径规划matlab程序代码实现...

基于蚁群算法的机器人路径规划matlab程序代码实现

说明： 基于蚁群算法的机器人路径规划。使用网格离散化的方法对带有障碍物的环境建模，使用邻接矩阵存储该环境，使得问题转化为蚁群算法寻找最短路径。

%% ---------------------------------------------------------------% 输入参数列表% G 地形图为01矩阵，如果为1表示障碍物% Tau 初始信息素矩阵(认为前面的觅食活动中有残留的信息素)% K 迭代次数(指蚂蚁出动多少波)% M 蚂蚁个数(每一波蚂蚁有多少个)% S 起始点(最短路径的起始点)% E 终止点(最短路径的目的点)% Alpha 表征信息素重要程度的参数% Beta 表征启发式因子重要程度的参数% Rho 信息素蒸发系数% Q 信息素增加强度系数%% 输出参数列表% ROUTES 每一代的每一只蚂蚁的爬行路线% PL 每一代的每一只蚂蚁的爬行路线长度% Tau 输出动态修正过的信息素%% --------------------变量初始化----------------------------------function m_main()ticG=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 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 1 1 0 0 0 0 0 0 0 0 0 0 0;0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0;0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;0 0 1 1 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 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 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; %利用矩阵来表示栅格MM=size(G1); % G 地形图为01矩阵，如果为1表示障碍物 返回矩阵G的行数(有几行)Tau=ones(MM*MMMM*MM);% Tau 初始信息素矩阵，在这里设置所有位置初始信息素都相同(400*400矩阵)Tau=8.*Tau;K=100; % K 迭代次数(指蚂蚁出动多少波)M=50; % M 蚂蚁个数(每一波蚂蚁有多少个)S=1 ; % S 起始点(最短路径的起始点)E=MM*MM; % E 终止点(最短路径的目的点)Alpha=1; % Alpha 表征信息素重要程度的参数Beta=7; % Beta 表征启发式因子重要程度的参数Rho=0.3; % Rho 信息素蒸发系数Q=1; % Q 信息素增加强度系数minkl=inf;mink=0;minl=0;D=G2D(G);N=size(D1);%N表示问题的规模a=1;%小方格象素的边长Ex=a*(mod(EMM)-0.5);%终止点横坐标mod求余函数if Ex==-0.5Ex=MM-0.5;endEy=a*(MM+0.5-ceil(E/MM));%终止点纵坐标cell返回大于或者等于指定表达式的最小整数Eta=zeros(N);%启发式信息，取为至目标点的直线距离的倒数%下面构造启发式信息矩阵for i=1:Nix=a*(mod(iMM)-0.5);if ix==-0.5ix=MM-0.5;endiy=a*(MM+0.5-ceil(i/MM));if i~=E %不等于Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;elseEta(i)=100;endendROUTES=cell(KM);%用细胞结构存储每一代的每一只蚂蚁的爬行路线里面的元素可以是任意类型的变量(属性变量，用户自己创建的变量)，而不必是统一的类型PL=zeros(KM);%用矩阵存储每一代的每一只蚂蚁的爬行路线长度%% -----------启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁--------------------for k=1:Kfor m=1:M%% 第一步：状态初始化W=S;%当前节点初始化为起始点Path=S;%爬行路线初始化PLkm=0;%爬行路线长度初始化TABUkm=ones(N);%禁忌表初始化TABUkm(S)=0;%已经在初始点了，因此要排除DD=D;%邻接矩阵初始化%% 第二步：下一步可以前往的节点DW=DD(W:);DW1=find(DW);for j=1:length(DW1)if TABUkm(DW1(j))==0DW(DW1(j))=0;endendLJD=find(DW);Len_LJD=length(LJD);%可选节点的个数%% 觅食停止条件：蚂蚁未遇到食物或者陷入死胡同while W~=E&&Len_LJD>=1%% 第三步：转轮赌法选择下一步怎么走PP=zeros(Len_LJD);for i=1:Len_LJDPP(i)=(Tau(WLJD(i))^Alpha)*((Eta(LJD(i)))^Beta);endsumpp=sum(PP); %不太理解？？？PP=PP/sumpp;%建立概率分布Pcum(1)=PP(1);for i=2:Len_LJDPcum(i)=Pcum(i-1)+PP(i);endSelect=find(Pcum>=rand);to_visit=LJD(Select(1));%% 第四步：状态更新和记录Path=[Pathto_visit];%路径增加PLkm=PLkm+DD(Wto_visit);%路径长度增加W=to_visit;%蚂蚁移到下一个节点for kk=1:Nif TABUkm(kk)==0DD(Wkk)=0;DD(kkW)=0;endendTABUkm(W)=0;%已访问过的节点从禁忌表中删除DW=DD(W:);DW1=find(DW);for j=1:length(DW1)if TABUkm(DW1(j))==0DW(j)=0;endendLJD=find(DW);Len_LJD=length(LJD);%可选节点的个数end%% 第五步：记下每一代每一只蚂蚁的觅食路线和路线长度ROUTES{km}=Path;if Path(end)==EPL(km)=PLkm;if PLkm