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

DFS和BFS

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

深度优先

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

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

/* 
 * 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. 重复这个步骤，直到遍历到根节点的最后一个子节点
5. 然后回到根节点的第一个子节点，将这个子节点作为根节点，重复 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: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（直边或者斜边），这时的迪杰斯特拉算法 = 广度优先搜索算法 = 低效率但高覆盖率