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;endend% 上节点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;endend% 右上节点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;endend% 左节点ifcol-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;endend% 右节点ifcol+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;endend% 左下节点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;endend% 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;endend% 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;endend
3.3、Floyd.m文件
具体的算法实现
% Floyd算法clcclearclose 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);% 所有节点间的距离mappath = 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];endendendend%% 进入三层主循环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)];endendendendendend%% 画栅格界面% 找出目标最优路径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、无人驾驶--路径规划算法：Floyd算法】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)];endendendendendend
此段代码是实现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、算法实现效果