3.1 Floyd路径规划算法原理

Floyd算法又称为插点法,是一种利用动态规划的思想寻找给定的加权图中多源点之间最短路径的算法,与Dijkstra算法类似。

  • 1,从任意一条单边路径开始。所有两点之间的距离是边的权,如果两点之间没有边相连,则权为无穷大。
  • 2,对于每一对顶点 u 和 v,看看是否存在一个顶点 w 使得从 u 到 w 再到 v 比已知的路径更短。如果是更新它。
  • 把图用邻接矩阵G表示出来,如果从Vi到Vj有路可达,则Gij=d,d表示该路的长度;否则Gij=无穷大。定义一个矩阵D用来记录所插入点的信息,Dij表示从Vi到Vj需要经过的点,初始化Dij=j。把各个顶点插入图中,比较插点后的距离与原来的距离,Gij = min( Gij, Gij+Gij ),如果Gij的值变小,则Dij=k。在G中包含有两点之间最短道路的信息,而在D中则包含了最短通路径的信息。
  • 比如,要寻找从V5到V1的路径。根据D,假如D(5,1)=3则说明从V5到V1经过V3,路径为{V5,V3,V1},如果D(5,3)=3,说明V5与V3直接相连,如果D(3,1)=1,说明V3与V1直接相连。

3.2 Floyd路径规划算法图论示例

在这里插入图片描述在这里插入图片描述在这里插入图片描述在这里插入图片描述
在这里插入图片描述

3.3 Floyd路径规划MATLAB代码

3.3.1 实例应用

例如,对于如图显示的点阵图,X长度为20,Y长度位15。黄色表示起点坐标为(1,1),紫色表示终点坐标(20,15)。障碍物坐标显示为黑色,其余为空地。规划从起点(1,1)到终点(20,15)的路径。调用代码:

在这里插入图片描述

3.3.2 主代码Floyd_main.m

%% 对地图数据进行初始化
clc;clear;
rows = 15;cols = 20;                            % 设置地图行列长度
startSub = [15,1];                              % 起点行列位置
goalSub = [1,20];                               % 终点行列位置
num_allNode = rows*cols;                        % 栅格节点总个数
dis = inf(num_allNode,num_allNode);             % 所有节点间的距离dis矩阵
path = inf(num_allNode,num_allNode);            % 路径矩阵

% 将行列值转化为线性索引值进行算法设计
startIndex = sub2ind([rows,cols],startSub(1),startSub(2));
goalIndex = sub2ind([rows,cols],goalSub(1),goalSub(2));
obsIndex = randi([1,300],100,1);


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

% 起点、终点、障碍物区域的权值
field(obsIndex) = 2;
field(startIndex) = 4;
field(goalIndex) = 5;

%% ***********************Floyd算法**********************************************
% 对路径矩阵进行初始化
for node = 1:num_allNode
    if field(node) ~= 2
        nextNodes = Floyd_NextNodes(rows, cols, node, field);
        for i = 1:8
            % 判断nextNode(1)主要防止边界点没有8个会有inf存在
            if ~(isinf(nextNodes(i,1)) || isinf(nextNodes(i,2)))
                next = nextNodes(i,1);
                % 初始化的dis矩阵,一行(起点->终点)的行列值,最多有8个是非inf(得到结果的)
                dis(node, next) = nextNodes(i,2);
            end
        end
    end
end

% 选择中间点依次更新距离矩阵和路径矩阵
for midpoint = 1:num_allNode
    for startpoint = 1:num_allNode
        for goalpoint = 1:num_allNode
            % 其中,起点不能等于终点,中间点不等于起点或者终点(扣去2(n-1)种情况)
            if startpoint ~= goalpoint && midpoint ~= startpoint && midpoint ~= goalpoint
                % 距离更小则更新距离矩阵和路径矩阵
                if dis(startpoint,midpoint) + dis(midpoint,goalpoint) < dis(startpoint,goalpoint)
                    dis(startpoint,goalpoint) = dis(startpoint,midpoint) + dis(midpoint,goalpoint);
                    path(startpoint,goalpoint) = midpoint;
                end
            end
        end
    end
end


%% *******求路径,采用递归前序遍历出结果
optPath = [];
optPath = Floyd_GetPath(path,startIndex,goalIndex,optPath);


%% 绘制地图
% 定义函数,列数,以及障碍物坐标
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);
image(1.5,1.5,field);

% 设置栅格属性
grid on;hold on;
set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
set(gca, 'XAxisLocation','top')
axis image;

% 绘制最短路径的折线图,将起点和终点添加进optPath路径中
[plotr,plotc] = ind2sub([rows,cols],[startIndex,optPath,goalIndex]);
plot(plotc+0.5,plotr+0.5,'LineWidth',2.5);



% % function  optpath = Floyd_GetPath(path,startpoint,goalpoint,optpath)
% % %FLOYD_GETPATH 前序遍历求出所有的节点
% % %% ****最关键****
% %         % 返回值optpath如果没有返回值,optpath默认每次传入都是[]
% %         % optpath作为返回值,才能使得遍历过程中的1、执行完之后的2、和3、都是一个optpath
% % 
% % midpoint = path(startpoint,goalpoint);
% % if ~isinf(midpoint)
% %     % 1、前序遍历,先遍历左节点
% %     optpath = Floyd_GetPath(path,startpoint,midpoint,optpath);
% %     % 2、存取结果,其实是在读取中间节点即根节点的值
% %     optpath(end+1) = midpoint;
% %     % 3、遍历右节点
% %     optpath = Floyd_GetPath(path,midpoint,goalpoint,optpath);
% % end
% % 
% % end

3.3.3 函数代码Floyd_NextNode.m

function nextNode = Floyd_NextNodes(rows,cols,nodeIndex,field)
%FL_NEXTNODES 注释见Dijkstra_NextNode函数,二者是一致的

[r,c] = ind2sub([rows,cols],nodeIndex);
nextNode = inf(8,2);
movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1];

for i = 1:8
    if 0 < r+movePos(i,1) && r+movePos(i,1) <= rows && 0 < c+movePos(i,2) && c+movePos(i,2) <= cols  
        subscript = [r+movePos(i,1),c+movePos(i,2)];
        index = sub2ind([rows,cols], subscript(1),subscript(2));
        nextNode(i,1) = index;
        if field(subscript(1), subscript(2)) ~= 2
            cost = norm(subscript - [r,c]);
            nextNode(i,2) = cost;
        end
    end
end
end

3.3.3 函数代码Floyd_GetPath.m

function  optpath = Floyd_GetPath(path,startpoint,goalpoint,optpath)
%FLOYD_GETPATH 前序遍历求出所有的节点
%% ****最关键****
        % 返回值optpath如果没有返回值,optpath默认每次传入都是[]
        % optpath作为返回值,才能使得遍历过程中的1、执行完之后的2、和3、都是一个optpath

midpoint = path(startpoint,goalpoint);
if ~isinf(midpoint)
    % 1、前序遍历,先遍历左节点
    optpath = Floyd_GetPath(path,startpoint,midpoint,optpath);
    % 2、存取结果,其实是在读取中间节点即根节点的值
    optpath(end+1) = midpoint;
    % 3、遍历右节点
    optpath = Floyd_GetPath(path,midpoint,goalpoint,optpath);
end

end

3.4 Floyd路径规划Python代码

3.4.1 实现例子

在这里插入图片描述

3.4.2 辅助函数PathPlanning.py

3 移动机器人路径规划(1- 栅格地图绘制)的第1.4.2节

3.4.3 Floyd路径规划Python代码

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import colors
import copy

import PathPlanning

'''
Floyd_NextNode(field,node)找寻周围节点路径函数,比MATLAB省略部分多余输入
'''
def Floyd_NextNode(field,node):
    rows = len(field)
    cols = len(field[0])
    movepos = [[-1, 1], [0, 1], [1, 1], [-1, 0],
               [1, 0], [-1, -1], [0, -1], [1, -1]]
    nextnodes = [[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf],
                [np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf]]

    # 将父节点装换成行列式位置信息,方便进行上下左右移动,并对存放子节点的数据进行初始化
    nodeSub = PathPlanning.ind2sub([rows,cols],node)

    ## 更新父节点朝着各个方向移动到子节点的距离
    for i in range(8):
        r = nodeSub[0] + movepos[i][0]
        c = nodeSub[1] + movepos[i][1]

        # 父节点有效移动范围,不能越界
        if -1 < r and r < rows and -1 < c and c < cols:
            nextnodes[i][0] = PathPlanning.sub2ind([rows,cols],r,c)

            # 子节点不为障碍物时,计算距离
            if field[r, c] != 2:
                dist = np.sqrt(movepos[i][0]*movepos[i][0]+movepos[i][1]*movepos[i][1])
                nextnodes[i][1] = dist

    return nextnodes


'''
# Floyd_GetPath(path,startpoint,endpoint,optpath)前序遍历求出所有的节点
# optpath作为返回值,才能使得遍历过程中的1、执行完之后的2、和3、都是一个optpath
'''
def Floyd_GetPath(path,startpoint,endpoint,optpath):
    midpoint = path[startpoint][endpoint]

    if midpoint != np.inf:
        midpoint = int(midpoint)
        Floyd_GetPath(path,startpoint,midpoint,optpath)
        optpath.append(midpoint)
        Floyd_GetPath(path,midpoint,endpoint,optpath)

    return optpath

# 地图信息
rows = 5
cols = 6
startSub = [2,1]
goalSub = [2,5]
obsSub = [[1,3],[2,3],[3,3]]


# 栅格地图属性
field = np.ones((rows, cols))

field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5

for i in range(len(obsSub)):
    field[obsSub[i][0], obsSub[i][1]] = 2


# 数据转换
startIndex = PathPlanning.sub2ind([rows,cols],startSub[0], startSub[1])
goalIndex = PathPlanning.sub2ind([rows,cols],goalSub[0], goalSub[1])

num_allNode = rows*cols

dis = np.zeros([num_allNode,num_allNode])
path = np.zeros([num_allNode,num_allNode])

# 由于后续比较长度以全0矩阵或者全1矩阵对路径加减影响很大将dis矩阵全化成全inf矩阵
for i in range(num_allNode):
    for j in range(num_allNode):
        dis[i][j] = np.inf
        path[i][j] = np.inf


'''
## ***********************Floyd算法**********************************************
# ==================第一段,对路径矩阵进行初始化
'''
for node in range(num_allNode):
    nodeSub = PathPlanning.ind2sub([rows,cols],node)
    if field[nodeSub[0]][nodeSub[1]] != 2:
        nextNodes = Floyd_NextNode(field,node)
        for i in range(8):
            # ****判断nextNode(1)主要防止边界点没有8个会有inf存在
            if nextNodes[i][0] != np.inf and nextNodes[i][1] != np.inf:
                nextnode = nextNodes[i][0]
                # ****初始化的dis矩阵,一行(起点->终点)的行列值,最多有8个是非inf(得到结果的)
                dis[node][nextnode] = nextNodes[i][1]


# ===============第二段,选择中间点依次更新距离矩阵和路径矩阵
for midpoint in range(num_allNode):
    for startpoint in range(num_allNode):
        for endpoint in range(num_allNode):
            # ***其中,起点不能等于终点,中间点不等于起点或者终点(扣去2(n-1)种情况)
            if startpoint != endpoint and midpoint != startpoint and midpoint != endpoint:
                # ****距离更小则更新距离矩阵和路径矩阵
                if dis[startpoint][midpoint] + dis[midpoint][endpoint] < dis[startpoint][endpoint]:
                    dis[startpoint][endpoint] = dis[startpoint][midpoint] + dis[midpoint][endpoint]
                    path[startpoint][endpoint] = midpoint


optPath = []

Floyd_GetPath(path,startIndex,goalIndex,optPath)


optPathSub = []
for i in range(len(optPath)):
    optPathSub.append(PathPlanning.ind2sub([rows,cols],optPath[i]))

for i in range(0,len(optPathSub),1):
    field[optPathSub[i][0]][optPathSub[i][1]] = 6


field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5

PathPlanning.DrawHeatMap(field)
plt.show()