3、无人驾驶--路径规划算法：Floyd算法
3、Floyd算法 1、算法简介
1.1、Floyd（佛洛依德）算法是解决给定的加权图中顶点间的最短路径的一种算法，可以正确处理有向图的最短路径问题。
1.2、特点：Floyd算法是一种动态规划算法，稠密图效果最佳，节点间的连接权值可正可负。此算法简单有效，由于三重循坏结构紧凑，对于稠密图，效果要高于算法。
1.3、优缺点：
优点：容易理解，可以算出任意两节点之间的最短距离，代码编写简单。
缺点：时间复杂度比较高，对于稀疏图将会生成稀疏矩阵，极大浪费了储存空间。
2、算法思路
2.1、将图的所有相邻节点对应的权值写入矩阵，不相邻的设为inf；
2.2、以A为中介点，分别更新B/C/D/E/F/G经过中介点A到其它节点的累积权值；
2.3、如上,以除A以外的节点为中介点，分别更新其它各点经过中介点到其它节点的累积权值；
最后得到一个全新的矩阵
3、算法具体实现 3.1、.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、.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-算法的时间复杂度为O(N3)，空间复杂度为O(N2)。
4、算法实现效果
