机器人路径规划1 Dijkstra

简介

在机器人研究领域，给定某一特定任务之后，如何规划机器人的运动方式至关重要。PathPlanning 是使用 Python 实现的存储库，实现了机器人技术中常用的路径规划算法。开发者还为每个算法设计了动画来演示运行过程，相当直观清晰。

PathPlanning
开源了！机器人技术常用的路径规划算法（含动画演示）
古月居 - 机器人路径规划之Dijkstra算法
运动规划入门 | 1. 白话Dijkstra，从原理到Matlab实现

Dijkstra 算法介绍

• 深度优先搜索（Depth First Search, DFS）
• 广度优先搜索（Breadth First Search, BFS）

Dijkstra 算法属于广度优先搜索算法，从一个节点遍历其余各节点的最短路径算法，解决的是有权图中最短路径问题
Dijkstra 算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到起始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止

defColorMap.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;

getNeighborNodes.m

%% 遍历每一个节点的临近节点
function neighborNodes = getNeighborNodes(rows, cols, lineIndex, field)
% 把数组中元素索引值转换为该元素在数组中对应的下标
[row, col] = ind2sub([rows, cols], lineIndex);          % 得到父节点的行列索引值
% 创建8行2列所有值都为inf的数组
neighborNodes = inf(8, 2);

%% 查找当前父节点临近的周围8个子节点
% 左上节点
if row - 1 > 0 && col - 1 > 0                           % 判断节点是否在边缘地带
    child_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;              % 将左上节点的线性索引值存入neighborNodes第1行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 左上节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(1, 2) = cost;                     % 将左上节点的代价值存入neighborNodes第1行第2列
    end
end

% 正上节点
if row - 1 > 0                                          % 判断节点是否在边缘地带
    child_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;              % 将正上节点的线性索引值存入neighborNodes第2行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 正上节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(2, 2) = cost;                     % 将正上节点的代价值存入neighborNodes第2行第2列
    end
end

% 右上节点
if row - 1 > 0 && col + 1 <= cols                       % 判断节点是否在边缘地带
    child_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;              % 将右上节点的线性索引值存入neighborNodes第3行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 右上节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(3, 2) = cost;                     % 将右上节点的代价值存入neighborNodes第3行第2列
    end
end
        
% 左节点
if col - 1 > 0                                          % 判断节点是否在边缘地带
    child_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;              % 将右上节点的线性索引值存入neighborNodes第4行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 右上节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(4, 2) = cost;                     % 将右上节点的代价值存入neighborNodes第4行第2列
    end
end

% 右节点
if col + 1 <= cols                                      % 判断节点是否在边缘地带
    child_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;              % 将右节点的线性索引值存入neighborNodes第5行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 右节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(5, 2) = cost;                     % 将右节点的代价值存入neighborNodes第5行第2列
    end
end

% 左下节点
if row + 1 <= rows && col - 1 > 0                       % 判断节点是否在边缘地带
    child_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;              % 将左下节点的线性索引值存入neighborNodes第6行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 左下节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(6, 2) = cost;                     % 将左下节点的代价值存入neighborNodes第6行第2列
    end
end

% 正下节点
if row + 1 <= rows                                      % 判断节点是否在边缘地带
    child_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;              % 将正下节点的线性索引值存入neighborNodes第7行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 正下节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(7, 2) = cost;                     % 将正下节点的代价值存入neighborNodes第7行第2列
    end
end

% 右下节点
if row + 1 <= rows && col + 1 <= cols                   % 判断节点是否在边缘地带
    child_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;              % 将右下节点的线性索引值存入neighborNodes第8行第1列
    if field(child_node_sub(1), child_node_sub(2)) ~= 2 % 右下节点不是障碍物
        cost = norm(child_node_sub - [row, col]);       % 简化利用norm求代价
        neighborNodes(8, 2) = cost;                     % 将右下节点的代价值存入neighborNodes第8行第2列
    end
end

Dijkstra.m

clc
clear
close all

%% 栅格界面、场景定义
% 行数和列数
rows = 20;
cols = 30;
[field, cmap] = defColorMap(rows, cols);

% 起点、终点、障碍物区域
startPos = 2;
goalPos = rows * cols - 2;
field(startPos) = 4;
field(goalPos) = 5;

%% 算法初始化
% S/U的第一列表示栅格节点线性索引编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更
U(:, 1) = (1 : rows * cols)';
U(:, 2) = inf;
S = [startPos, 0];
U(startPos, :) = [];                            % 删除起始节点信息

% 更新起点的邻节点及代价
neighborNodes = getNeighborNodes(rows, cols, startPos, field);
for i = 1 : 8
    childNode = neighborNodes(i, 1);            % 将线性索引值存入childNode
    
    % 判断该子节点是否存在
    if ~isinf(childNode)                        % 如果子节点存在
        idx = find(U(:, 1) == childNode);       % 返回存在子节点的线性索引值
        U(idx, 2) = neighborNodes(i, 2);        % 将代价更新传入U中
    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

%% 循环遍历
while ~isempty(U)
    
    % 在U集合中找出当前最小距离值的节点，视为父节点，并移除该节点至S集合中
    [dist_min, idx] = min(U(:, 2));             % 返回最小值和下标
    parentNode = U(idx, 1);
    S(end + 1, :) = [parentNode, dist_min];
    U(idx, :) = [];
    
    % 获得当前节点的临近子节点
    neighborNodes = getNeighborNodes(rows, cols, parentNode, field);
    
    % 依次遍历邻近子节点，判断是否在U集合中更新邻近节点的距离值
    for i = 1 : 8
        % 需要判断的子节点
        childNode = neighborNodes(i, 1);        % 将线性索引值存入childNode
        cost = neighborNodes(i, 2);
        % 判断该子节点是否存在
        if ~isinf(childNode) && ~ismember(childNode, S)
            idx_U = find(childNode == U(:, 1));   % 找出U集合节点childNode的索引值
            
            % 判断是否更新
            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;

% 画栅格图
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;