运动规划(三) Floyd算法(Matlab)

主函数

clc
clear
close all

rows=10;cols=20;
field=ColorMap(rows,cols);

startPos=2;goalPos=rows*cols-2;
field(startPos)=4;
field(goalPos)=5;

n=rows*cols;
map=inf(n,n);
for i=1:n
    map(i,i)=0;
end
path = cell(n, n);  % 存放对应的路径
for startNode=1:n
    neighborNodes=NeighborNodes(rows,cols,startNode,field);
    
    for i=1:8
        if ~(isinf(neighborNodes(i,1)) || isinf(neighborNodes(i,2)))
            map(startNode,neighborNodes(i,1))=neighborNodes(i,2);
            path{startNode,neighborNodes(i,1)}=[startNode,neighborNodes(i,1)];
        end
    end
end



for i=1:n
    for j=1:n
        if j ~=i
            for k=1:n
                if k ~=j && k ~=i
                    if map(j,i)+map(i,k)<map(j,k)
                        map(j,k)=map(j,i)+map(i,k);
                        path{j,k}=[path{j,i},path{i,k}(2:end)];
                    end
                end
            end
        end
    end
end

path_opt=path{startPos,goalPos};
field(path_opt(2:end-1))=6;

image(1.5,1.5,field);
grid on;
set(gca,'gridcolor','k','gridline','-','linewidth',2,'gridalpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
    

ColorMap 栅格地图绘制函数

function field=ColorMap(rows,cols)
cMap=[1,1,1;  %白色  1 空白栅格
      0 0 0;  %黑色  2  障碍物
      1 0 0;  %红色  3
      1 1 0;  %黄色  4  起点
      1 0 1;  %晶红  5  终点
      0 1 0;  %绿色  6  规划路径
      0 1 1];  %青色  7  动态路径
colormap(cMap);

field=ones(rows,cols);
obsRate=0.3;
obsNum=floor(rows*cols*obsRate);
obsInd=randi([1,rows*cols],obsNum,1);
field(obsInd)=2;

end

NeighborNodes 子节点扩展函数

function neighborNodes=NeighborNodes(rows,cols,lineInd,field)
    
[row,col]=ind2sub([rows,cols],lineInd);
neighborNodes=inf(8,2);

%左上子节点
if row-1>0 && col-1>0
    child_node_sub=[row-1,col-1];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(1,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(1,2)=norm([row,col]-child_node_sub);
    end
end

%上子节点
if row-1>0 
    child_node_sub=[row-1,col];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(2,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(2,2)=norm([row,col]-child_node_sub);
    end
end

%右上子节点
if row-1>0  && col+1<=cols
    child_node_sub=[row-1,col+1];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(3,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(3,2)=norm([row,col]-child_node_sub);
    end
end

%左子节点
if  col-1>0
    child_node_sub=[row,col-1];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(4,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(4,2)=norm([row,col]-child_node_sub);
    end
end

%右子节点
if  col+1<=cols
    child_node_sub=[row,col+1];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(5,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(5,2)=norm([row,col]-child_node_sub);
    end
end

%左下子节点
if  row+1<=rows && col-1>0
    child_node_sub=[row+1,col-1];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(6,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(6,2)=norm([row,col]-child_node_sub);
    end
end

%下子节点
if  row+1<=rows 
    child_node_sub=[row+1,col];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(7,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(7,2)=norm([row,col]-child_node_sub);
    end
end

%右下子节点
if  row+1<=rows && col+1<=cols
    child_node_sub=[row+1,col+1];
    child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
     neighborNodes(8,1)=child_node_ind;
    if field( child_node_ind)~=2
        neighborNodes(8,2)=norm([row,col]-child_node_sub);
    end
end
end
% function neighborNodes=NeighborNodes(rows,cols,lineInd,field)
% 
% [row,col]=ind2sub([rows,cols],lineInd);
% neighborNodes=inf(8,2);
% 
% %左上子节点
% if row-1>0 && col-1>0
%     child_node_sub=[row-1,col-1]; 
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(1,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(1,2)=cost;
%     end
% end
% 
% %上子节点
% if row-1>0 
%     child_node_sub=[row-1,col];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(2,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(2,2)=cost;
%     end
% end
% 
% %右上子节点
% if row-1>0 && col+1 <= cols 
%     child_node_sub=[row-1,col+1];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(3,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(3,2)=cost;
%     end
% end
% 
% %左子节点
% if  col-1 >0
%     child_node_sub=[row,col-1];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(4,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(4,2)=cost;
%     end
% end
% 
% %右子节点
% if  col+1 <= cols
%     child_node_sub=[row,col+1];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(5,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(5,2)=cost;
%     end
% end
% 
% %左下子节点
% if  row+1<=rows && col-1>0
%     child_node_sub=[row+1,col-1];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(6,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(6,2)=cost;
%     end
% end
% 
% %下子节点
% if  row+1<=rows 
%     child_node_sub=[row+1,col];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(7,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(7,2)=cost;
%     end
% end
% 
% %左下子节点
% if  row+1<=rows && col+1<=cols 
%     child_node_sub=[row+1,col+1];
%     child_node_ind=sub2ind([rows,cols],child_node_sub(1),child_node_sub(2));
%     neighborNodes(8,1)=child_node_ind;
%     if field(child_node_ind)~=2  % 不是障碍物
%         cost=norm([child_node_sub(1),child_node_sub(2)]-[row,col]);
%         neighborNodes(8,2)=cost;
%     end
% end
% end