3、Floyd算法

1、算法简介

1.1、Floyd（佛洛依德）算法是解决给定的加权图中顶点间的最短路径的一种算法，可以正确处理有向图的最短路径问题。

1.2、特点：Floyd算法是一种动态规划算法，稠密图效果最佳，节点间的连接权值可正可负。此算法简单有效，由于三重循坏结构紧凑，对于稠密图，效果要高于Dijkstra算法。

1.3、优缺点：

优点：容易理解，可以算出任意两节点之间的最短距离，代码编写简单。

缺点：时间复杂度比较高，对于稀疏图将会生成稀疏矩阵，极大浪费了储存空间。

2、算法思路

2.1、将图的所有相邻节点对应的权值写入矩阵，不相邻的设为inf；

2.2、以A为中介点，分别更新B/C/D/E/F/G经过中介点A到其它节点的累积权值；

2.3、如上,以除A以外的节点为中介点，分别更新其它各点经过中介点到其它节点的累积权值；

最后得到一个全新的矩阵

3、算法具体实现

3.1、defColorMap.m文件

作用：生成栅格图

function [field,cmap] = defColorMap(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-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);

% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);

% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

3.2、getNeighborNodes.m文件

作用：搭建个节点之间的关系；实现查找当前父节点临近的周围8个子节点

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

%% 查找当前父节点临近的周围8个子节点
% 左上节点
if row-1 > 0 && col-1 > 0
    child_node_sub = [row-1, col-1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(1,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(1,2) = cost;
    end
end

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

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

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

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

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

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

% 8.右下节点
if row+1 <= rows && col+1 <= cols
    child_node_sub = [row+1, col+1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(8,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(8,2) = cost;
    end
end

3.3、Floyd.m文件

具体的算法实现

% Floyd算法
clc
clear
close all

%% 栅格界面、场景定义
% 行数和列数
rows = 10;
cols = 20;
[field,cmap] = defColorMap(rows, cols);

% 起点、终点、障碍物区域
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;

%% 算法初始化
n = rows*cols;      % 栅格节点总个数
map = inf(n,n);     % 所有节点间的距离map
path = cell(n, n);  % 存放对应的路径
for startNode = 1:n
    if field(startNode) ~= 2
        neighborNodes = getNeighborNodes(rows, cols, startNode, field);
        for i = 1:8
            if ~(isinf(neighborNodes(i,1)) || isinf(neighborNodes(i,2)))
                neighborNode = neighborNodes(i,1);
                map(startNode, neighborNode) = neighborNodes(i,2);
                path{startNode, neighborNode} = [startNode, neighborNode];
            end
        end
    end
end
           
%% 进入三层主循环
for i = 1:n
    for j =  1:n
        if j ~= i
            for k =  1:n
                if k ~= i && k ~= j
                    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_target = path{startPos,goalPos};
field(path_target(2:end-1)) = 6;

% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;

3.3.1、重要代码解析

%% 进入三层主循环
for i = 1:n
    for j =  1:n
        if j ~= i
            for k =  1:n
                if k ~= i && k ~= j
                    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

此段代码是实现Floyd算法的三重循环

第一次循环：初始化矩阵i=1；判断 i 是否大于 n；否——则以结点i伟中介点，依次选择除i之外的每一个节点j（初始化j = 1）作为起始节点.

第二次循环：判断起始节点是否遍历完；否——以作为中介点、j为起始节点，依次选择除i、j外的每一个节点k作为目标节点。

第三次循环：判断目标结点K是否遍历完；否——判断起始节点j到中间节点i的距离+中间节点i到目标节点k的距离是否小于起始节点到目标节点的距离，是——则更新起始节点到目标节点的距离。

3.4、Floyd算法复杂度分析

Floyd-Warshall算法的时间复杂度为O(N3)，空间复杂度为O(N2)。

4、算法实现效果

想要获取工程源码，可以关注公众号：Kevin的学习站，后台回复：“Floyd”即可获取；希望此文对您有一定的帮助，整理不易，但您的点赞、关注、收藏就是对我最大的鼓励！