机器人路径规划之迪杰斯特拉算法（二）

DFS和BFS

说迪杰斯特拉算法之前，先为大家简单介绍两种常见的图论搜索算法，深度优先（DFS），广度优先（BFS）。

深度优先

深度优先就是从根节点开始，找到它的第一个子节点
在找到第一个子节点的子节点，依次往下递推直到当前节点没有其他的子节点
返回当前子节点的父节点，寻找有没有另外的子节点，若没有则一直重复这个步骤，直到找到目标点或者遍历完整个地图。

学过二叉树的同学激动地拍了拍大腿，这不就是前序遍历嘛，那这个遍历的方式我们可以简单的写成：

/* * describe  : 前序便利* parameter : bt_node 指向根节点的指针*             id 查找到的节点ID*             pre_node 指向当前找到的节点* return    : 指向id符合的节点的指针*/
static void ErgodicTree(BTNode* bt_node)
{if(bt_node == NULL){//        DEBUG("ErgodicTree bt_node == NULL \r\n");return;}/***** 这里查看当前节点信息，比如判断当前节点是否为终点等 *****//*********************************************************/ErgodicTree(bt_node->firstchild); ErgodicTree(bt_node->rightsib);
}// describe  : 前序遍历，执行所有叶子结点的函数指针指向
// parameter : tree 树的地址
// return    : None
void DisBehaviorTree(const BTTree* tree)
{if(tree == NULL){DEBUG("DisBehaviorTree error \r\n");return;}ErgodicTree(tree->root);
}

这里用的是递归的方式，当地图比较大时，频繁递归频繁出入栈会增加算法开销，比较常见的方式是用栈模拟递归算法，比如找到子节点时将子节点push到栈里，直到找不到子节点时，一路pop直到有新的子节点。非递归实现网上很多资料，这里不多赘述。

广度优先

广度优先和深度优先思路不太一样，深度优先是一路找到终端节点（无任何子节点的节点），然后一路返回找新的终端节点，且满足左中右的遍历顺序。重复这个步骤即可。

广度优先是找到当前节点下的所有子节点，并且依次遍历完毕，然后找到根节点第一个子节点，遍历这个子节点下的所有子节点，然后回到根节点的第二个子节点，遍历这个子节点下的所有子节点。重复这个步骤，直到遍历到根节点的最后一个子节点，直到

广度优先是找到当前节点下的所有子节点，并且依次遍历完毕
然后找到根节点第一个子节点，遍历这个子节点下的所有子节点
然后回到根节点的第二个子节点，遍历这个子节点下的所有子节点
重复这个步骤，直到遍历到根节点的最后一个子节点
然后回到根节点的第一个子节点，将这个子节点作为根节点，重复 1 2 3 4 步骤，直到图中所有节点遍历完毕，或者找到目标点。

迪杰斯特拉算法

算法思想

迪杰斯特拉算法是在广度优先搜索算法的基础上，加上了贪心算法的思想，即遍历完当前节点的所有子节点后，不从第一个子节点开始进行遍历，而是从这些子节点中，路径代价最少的节点开始进行遍历，且每一次切换根节点时，都进行路径代价最少这个约束条件的判断。

PS:路径代价一般是根据根节点到该节点的最短距离，反映拓扑地图中就是边的长度，根节点的路径代价肯定是0，其他节点的路径代价，则是从根节点到该节点的最短距离，以不同的路径到这个节点的距离是不同的，所以最短距离是时刻在更新的。

matlab实现

% 基于栅格地图的机器人规划路径算法
% 第二节：迪杰斯特拉算法
clc
clear
close%% 栅格界面、场景定义
% 行数和列数
rows = 10;
cols = 20;
[field,cmap] = defColorMap(rows,cols);% 起点、终点、障碍物区域
startPos = 2;
goalPos = 198;
field(startPos) = 4;
field(goalPos) = 5;%% 算法初始化
% S/U的第一列表示栅格地图节点线性索引编号
% 对于S，第二列表示从源节点到本节点以求得的最小距离，不在变更
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更
U(:,1) = (1:rows*cols);
U(:,2) = inf;
S = [startPos,0];   % 把起点装进U集合中
U(startPos,:) = []; % 删除Ｕ集合中的起始节点信息% 更新起点的邻接点及代价
neighborNodes = getNeighborNodes(rows,cols,startPos,field);
for i = 1:8childNode = neighborNodes(i,1);% 判断孩子节点是否存在if ~isinf(childNode)idx = find(U(:,1) == childNode); % 找到在U集合中相等的线性索引U(idx,2) = neighborNodes(i,2);end
end% S集合的最优路径集合
for i = 1 : rows*colspath{i,1} = i;
end
for i = 1:8childNode = neighborNodes(i,1);if ~isinf(neighborNodes(i,2)) % 代价不为空path{childNode,2} = [startPos,neighborNodes(i,1)]; % 存放从起点到其他子节点的坐标，[起点坐标，子节点坐标]（均为线性索引）end
endisEndLoop = false;while ~isempty(U) && ~isEndLooppause(0.1);% 在U集合中找出当前最小距离值的节点，视为父节点，并移除该节点至S集合中[dist_min,idx] = min(U(:,2));parentNode = U(idx,1);S(end+1,:) = [parentNode,dist_min];U(idx,:) = [];if parentNode == goalPosisEndLoop = true;end% 获取当前节点的临近节点neighborNodes = getNeighborNodes(rows,cols,parentNode,field);% 依次遍历邻近子节点，判断是否在U集合中更新邻近节点的距离值for i = 1:8% 需要判断的子节点childNode = neighborNodes(i,1);cost = neighborNodes(i,2);if ~isinf(childNode) && ~ismember(childNode, S)% 找出U集合中节点childNode的索引值idx_U = find(childNode == U(:,1));if field(U(idx_U,1)) ~=  2 && field(U(idx_U,1)) ~=  4 && field(U(idx_U,1)) ~=  5field(U(idx_U,1)) = 7;end% 画栅格图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;% 判断是否需要更新if dist_min + cost < U(idx_U,2);U(idx_U,2) = dist_min + cost;% 更新最优路径path{childNode,2} = [path{parentNode,2},childNode];endendend
end%% 画栅格界面
% 找出目标最优路径
path_opt = path{goalPos,2};
field(path_opt(2:end-1)) = 6;% 画出探索过的区域
for i = 1 : length(S)if ~isinf(S(i,2)) && S(i,1) ~= startPos && S(i,1) ~= goalPos && field(S(i,1)) ~= 6field(S(i,1)) = 7;end
end% 画栅格图
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;

画出该算法的动态演示图：

可以看到在不同场景的栅格地图下，迪杰斯特拉算法都能准确地找到终点，并且算出最短路径。

总结

对于在栅格场景下的迪杰斯特拉算法来讲，贪心策略基本宣告无效，因为每个节点之间的距离都是固定的1/1.4（直边或者斜边），这时的迪杰斯特拉算法 = 广度优先搜索算法 = 低效率但高覆盖率