最少点字典序最短路径_【优化求解】基于粒子群之机器人栅格路径规划

首先用栅格法描述机器人工作环境,在此基础上,将机器人路径表示为粒子位置的二进制编码,并以路径长度为适应值,产生初始种群后,再对粒子位置和速度进行更新,经过多次迭代,即可获得从起始点到目标点的一条全局最优路径.

clc;close allclearload('data4.mat')figure(1)%画障碍图hold onS=(S_coo(2)-0.5)*num_shange+(S_coo(1)+0.5);%起点对应的编号E=(E_coo(2)-0.5)*num_shange+(E_coo(1)+0.5);%终点对应的编号for i=1:num_shange    for j=1:num_shange        if sign(i,j)==1            y=[i-1,i-1,i,i];            x=[j-1,j,j,j-1];            h=fill(x,y,'k');            set(h,'facealpha',0.5)        end        s=(num2str((i-1)*num_shange+j));        %text(j-0.95,i-0.5,s,'fontsize',6)     endendaxis([0 num_shange 0 num_shange])%限制图的边界plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点set(gca,'YDir','reverse');%图像翻转for i=1:num_shange    plot([0 num_shange],[i-1 i-1],'k-');    plot([i i],[0 num_shange],'k-');%画网格线endPopSize=20;%种群大小OldBestFitness=0;%旧的最优适应度值gen=0;%迭代次数maxgen =20;%最大迭代次数c1=0.5;%认知系数c2=0.7;%社会学习系数w=0.96;%惯性系数%%%初始化路径w_min=0.5;w_max=1;Group=ones(num_point,PopSize);  %种群初始化for i=1:PopSize    p_lin=randperm(num_point)';%随机生成1*400不重复的行向量    %% 将起点编号放在首位    index=find(p_lin==S);    lin=p_lin(1);    p_lin(1)=p_lin(index);    p_lin(index)=lin;    Group(:,i)=p_lin;    %%将每个个体进行合理化处理    [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);    while flag==1%如处理不成功，则初始化个体，重新处理        p_lin=randperm(num_point)';        index=find(p_lin==S);        lin=p_lin(1);        p_lin(1)=p_lin(index);        p_lin(index)=lin;        Group(:,i)=p_lin;        [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);           endend%最优解route=Group(:,end)';index1=find(route==E);route_lin=route(1:index1);for i=2:index1    Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];    Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];    plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'b-.','LineWidth',3);hold on    endtitle('粒子群算法-随机路线');title('粒子群算法-随机路线');figure(2)hold onfor i=1:num_shange    for j=1:num_shange        if sign(i,j)==1            y=[i-1,i-1,i,i];            x=[j-1,j,j,j-1];            h=fill(x,y,'k');            set(h,'facealpha',0.5)        end        s=(num2str((i-1)*num_shange+j));        text(j-0.95,i-0.5,s,'fontsize',6)     endendaxis([0 num_shange 0 num_shange])%限制图的边界plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点set(gca,'YDir','reverse');%图像翻转for i=1:num_shange    plot([0 num_shange],[i-1 i-1],'k-');    plot([i i],[0 num_shange],'k-');%画网格线endfor i=2:index1    Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];    Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];    plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'b-.','LineWidth',3)end%初始化粒子速度(即交换序)Velocity =zeros(num_point,PopSize);   for i=1:PopSize    Velocity(:,i)=round(rand(1,num_point)'*num_point/10); %round取整end%计算每个个体对应路径的距离for i=1:PopSize     EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);endIndivdualBest=Group;%记录各粒子的个体极值点位置,即个体找到的最短路径IndivdualBestFitness=EachPathDis;%记录最佳适应度值,即个体找到的最短路径的长度[GlobalBestFitness,index]=min(EachPathDis);%找出全局最优值和相应序号%寻优while gen < maxgen    w=w_max-(w_max-w_min)*gen/maxgen;    %迭代次数递增    gen = gen +1    %更新全局极值点位置,这里指路径    for i=1:PopSize           GlobalBest(:,i) = Group(:,index);    end       %求pij-xij ,pgj-xij交换序，并以概率c1，c2的保留交换序    pij_xij=GenerateChangeNums(Group,IndivdualBest);  %根据个体最优解求交换序    pij_xij=HoldByOdds(pij_xij,c1);%以概率c1保留交换序    pgj_xij=GenerateChangeNums(Group,GlobalBest);%根据全局最优解求交换序    pgj_xij=HoldByOdds(pgj_xij,c2);%以概率c2保留交换序       %以概率w保留上一代交换序    Velocity=HoldByOdds(Velocity,w);    Group = PathExchange(Group,Velocity); %根据交换序进行路径交换    Group = PathExchange(Group,pij_xij);    Group = PathExchange(Group,pgj_xij);    for i = 1:PopSize            [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);        while flag==1            p_lin=randperm(num_point)';            index=find(p_lin==S);            lin=p_lin(1);            p_lin(1)=p_lin(index);            p_lin(index)=lin;            Group(:,i)=p_lin;            [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);        end        end     for i = 1:PopSize    % 更新各路径总距离        EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);    end    IsChange = EachPathDis    IndivdualBest(:, find(IsChange)) = Group(:, find(IsChange));%更新个体最佳路径    IndivdualBestFitness = IndivdualBestFitness.*( ~IsChange) + EachPathDis.*IsChange;%更新个体最佳路径距离    [GlobalBestFitness, index] = min(IndivdualBestFitness);%更新全局最佳路径,记录相应的序号    if GlobalBestFitness~=OldBestFitness %比较更新前和更新后的适应度值;        OldBestFitness=GlobalBestFitness;%不相等时更新适应度值        best_route=IndivdualBest(:,index)';    end        BestFitness(gen) =GlobalBestFitness;%每一代的最优适应度end%最优解index1=find(best_route==E);route_lin=best_route(1:index1);for i=2:index1    Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];    Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];    plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3)endfor i=1:PopSize    p_lin=randperm(num_point)';%随机生成1*400不重复的行向量    %% 将起点编号放在首位    index=find(p_lin==S);    lin=p_lin(1);    p_lin(1)=p_lin(index);    p_lin(index)=lin;    Group(:,i)=p_lin;    %%将每个个体进行合理化处理    [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);    while flag==1%如处理不成功，则初始化个体，重新处理        p_lin=randperm(num_point)';        index=find(p_lin==S);        lin=p_lin(1);        p_lin(1)=p_lin(index);        p_lin(index)=lin;        Group(:,i)=p_lin;        [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);           endend%最优解route=Group(:,end)';index3=find(route==E);route_lin1=route(1:index3);for i=2:index3    Q1=[mod(route_lin1(i-1)-1,num_shange)+1-0.5,ceil(route_lin1(i-1)/num_shange)-0.5];    Q2=[mod(route_lin1(i)-1,num_shange)+1-0.5,ceil(route_lin1(i)/num_shange)-0.5];    plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'c-.','LineWidth',3);hold onendfor i=1:PopSize    p_lin=randperm(num_point)';%随机生成1*400不重复的行向量    %% 将起点编号放在首位    index=find(p_lin==S);    lin=p_lin(1);    p_lin(1)=p_lin(index);    p_lin(index)=lin;    Group(:,i)=p_lin;    %%将每个个体进行合理化处理    [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);    while flag==1%如处理不成功，则初始化个体，重新处理        p_lin=randperm(num_point)';        index=find(p_lin==S);        lin=p_lin(1);        p_lin(1)=p_lin(index);        p_lin(index)=lin;        Group(:,i)=p_lin;        [Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);           endend%最优解route=Group(:,end)';index2=find(route==E);route_lin2=route(1:index2);for i=2:index2    Q1=[mod(route_lin2(i-1)-1,num_shange)+1-0.5,ceil(route_lin2(i-1)/num_shange)-0.5];    Q2=[mod(route_lin2(i)-1,num_shange)+1-0.5,ceil(route_lin2(i)/num_shange)-0.5];    plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'m-.','LineWidth',3);hold onendtitle('粒子群算法-对比路线');figure(3)hold onfor i=1:num_shange    for j=1:num_shange        if sign(i,j)==1            y=[i-1,i-1,i,i];            x=[j-1,j,j,j-1];            h=fill(x,y,'k');            set(h,'facealpha',0.5)        end        s=(num2str((i-1)*num_shange+j));        text(j-0.95,i-0.5,s,'fontsize',6)     endendaxis([0 num_shange 0 num_shange])%限制图的边界plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点set(gca,'YDir','reverse');%图像翻转for i=1:num_shange    plot([0 num_shange],[i-1 i-1],'k-');    plot([i i],[0 num_shange],'k-');%画网格线endfor i=2:index1    Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];    Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];    plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3)endtitle('粒子群算法-最优路线');%进化曲线figure(4);plot(BestFitness);xlabel('迭代次数')ylabel('适应度值')grid on;title('进化曲线');disp('粒子群算法-最优路线方案：')disp(num2str(route_lin))disp(['起点到终点的距离:',num2str(BestFitness(end))]);figure(5);plot(BestFitness*100);xlabel('迭代次数')ylabel('适应度值')grid on;title('最佳个体适应度值变化趋势');

function [path, fitness] = fun_dijstra( pop, A, dij )path =[];for i=1:length(pop)-1%     path1 = find_path2(pop(i), pop(i+1), A);  % 找路径    path1 = dijkstra(pop(i), pop(i+1), dij);  % 找路径    path = [path,path1];end% 删除重复的节点index=[];for i=1:length(path)-1    if(path(i)==path(i+1))        index=[index,i];    endendpath(index)=[];fitness = ca_tsp(path,dij);

function [r_path, r_cost] = dijkstra(pathS, pathE, transmat)% The Dijkstra's algorithm, Implemented by Yi Wang, 2005%   pathS: 所求最短路径的起点%   pathE :所求最短路径的终点%   transmat: 图的转移矩阵或者邻接矩阵，应为方阵if ( size(transmat,1) ~= size(transmat,2) )  error( 'detect_cycles:Dijkstra_SC', ...         'transmat has different width and heights' );end% 初始化:%  noOfNode-图中的顶点数%  parent(i)-节点i的父节点%  distance(i)-从起点pathS的最短路径的长度%  queue-图的广度遍历noOfNode = size(transmat, 1);for i = 1:noOfNode  parent(i) = 0;       % 初值操作  distance(i) = Inf;    % 初值操作endqueue = [];           % 队列% 由路径开始最短路计算for i=1:noOfNode  if transmat(pathS, i)~=Inf    distance(i) = transmat(pathS, i);    parent(i)   = pathS;  % 当前路径    queue     = [queue i];  endend% 对图进行广度遍历while length(queue) ~= 0  hopS  = queue(1);  queue = queue(2:end);    for hopE = 1:noOfNode    if distance(hopE) > distance(hopS) + transmat(hopS,hopE)   % 如果当前距离大于转换后的距离      distance(hopE) = distance(hopS) + transmat(hopS,hopE);  % 更新      parent(hopE)   = hopS;      queue         = [queue hopE];    end  end  end% 回溯进行最短路径的查找r_path = [pathE];   i = parent(pathE);while i~=pathS && i~=0  r_path = [i r_path];  i      = parent(i);endif i==pathS  r_path = [i r_path];   % 记录else  r_path = []          % 清空end% 返回最短路径的权和r_cost = distance(pathE);

% ca_tsp.m% 计算路径长度function ltsp=ca_tsp(c,dij)    i=1;    ltsp=0;    while i        ltsp=ltsp+dij(c(i),c(i+1));        i=i+1;    endend

往期回顾>>>>>>

【模式识别】Matlab指纹识别【优化求解】A*算法解决三维路径规划问题【优化求解】模拟退火遗传实现带时间窗的车辆路径规划问题【数学建模】Matlab实现SEIR模型【优化求解】基于NSGA-2的求解多目标柔性车间调度算法 【优化求解】蚁群算法求最优值  【图像处理】粒子群算法结合模糊聚类分割算法实现图像的分割 【优化算法】基于粒子群算法的充电站最优布局 【优化求解】粒子群算法之电线布局优化 【优化求解】遗传算法解决背包问题 完整代码输入 源代码