说迪杰斯特拉算法之前,先为大家简单介绍两种常见的图论搜索算法,深度优先(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直到有新的子节点。非递归实现网上很多资料,这里不多赘述。
广度优先和深度优先思路不太一样,深度优先是一路找到终端节点(无任何子节点的节点),然后一路返回找新的终端节点,且满足左中右的遍历顺序。重复这个步骤即可。
广度优先是找到当前节点下的所有子节点,并且依次遍历完毕,然后找到根节点第一个子节点,遍历这个子节点下的所有子节点,然后回到根节点的第二个子节点,遍历这个子节点下的所有子节点。重复这个步骤,直到遍历到根节点的最后一个子节点,直到
迪杰斯特拉算法是在广度优先搜索算法的基础上,加上了贪心算法的思想,即遍历完当前节点的所有子节点后,不从第一个子节点开始进行遍历,而是从这些子节点中,路径代价最少的节点开始进行遍历,且每一次切换根节点时,都进行路径代价最少这个约束条件的判断。
PS:路径代价一般是根据根节点到该节点的最短距离,反映拓扑地图中就是边的长度,根节点的路径代价肯定是0,其他节点的路径代价,则是从根节点到该节点的最短距离,以不同的路径到这个节点的距离是不同的,所以最短距离是时刻在更新的。
% 基于栅格地图的机器人规划路径算法
% 第二节:迪杰斯特拉算法
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,:) = []; % 删除U集合中的起始节点信息
% 更新起点的邻接点及代价
neighborNodes = getNeighborNodes(rows,cols,startPos,field);
for i = 1:8
childNode = 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*cols
path{
i,1} = i;
end
for i = 1:8
childNode = neighborNodes(i,1);
if ~isinf(neighborNodes(i,2)) % 代价不为空
path{
childNode,2} = [startPos,neighborNodes(i,1)]; % 存放从起点到其他子节点的坐标,[起点坐标,子节点坐标](均为线性索引)
end
end
isEndLoop = false;
while ~isempty(U) && ~isEndLoop
pause(0.1);
% 在U集合中找出当前最小距离值的节点,视为父节点,并移除该节点至S集合中
[dist_min,idx] = min(U(:,2));
parentNode = U(idx,1);
S(end+1,:) = [parentNode,dist_min];
U(idx,:) = [];
if parentNode == goalPos
isEndLoop = 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)) ~= 5
field(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];
end
end
end
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)) ~= 6
field(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(直边或者斜边),这时的迪杰斯特拉算法 = 广度优先搜索算法 = 低效率但高覆盖率