3、无人驾驶

3、无人驾驶

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 > 0child_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(1,2) = cost;end
end% 上节点
if row-1 > 0child_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(2,2) = cost;end
end% 右上节点
if row-1 > 0 && col+1 <= colschild_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(3,2) = cost;end
end% 左节点
if  col-1 > 0child_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(4,2) = cost;end
end% 右节点
if  col+1 <= colschild_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(5,2) = cost;end
end% 左下节点
if row+1 <= rows && col-1 > 0child_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(6,2) = cost;end
end% 7.下节点
if row+1 <= rowschild_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)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(7,2) = cost;end
end% 8.右下节点
if row+1 <= rows && col+1 <= colschild_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)) ~= 2cost = 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:nif field(startNode) ~= 2neighborNodes = getNeighborNodes(rows, cols, startNode, field);for i = 1:8if ~(isinf(neighborNodes(i,1)) || isinf(neighborNodes(i,2)))neighborNode = neighborNodes(i,1);map(startNode, neighborNode) = neighborNodes(i,2);path{startNode, neighborNode} = [startNode, neighborNode];endendend
end%% 进入三层主循环
for i = 1:nfor j =  1:nif j ~= ifor k =  1:nif k ~= i && k ~= jif 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)];endendendendend
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:nfor j =  1:nif j ~= ifor k =  1:nif k ~= i && k ~= jif 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)];endendendendend
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”即可获取；希望此文对您有一定的帮助，整理不易，但您的点赞、关注、收藏就是对我最大的鼓励！