《Motion Planning》【1】Dijkstra算法实现

运行环境MatlabR2022a

注：原理可自行Google,这里只有代码实现。

        关于Matlab坐标转换的问题自行Mathworks。

1.Dijkstra_main.m文件

clear;clc;close all;
rows = 20;
cols = 20;
sz = [rows cols];
start_sub = [20 1]; % 起始点
goal_sub = [1 20]; % 终点

%障碍物
%load写法
load obs_sub.mat -ascii % 这里面写障碍物的坐标（自己固定障碍物）
%随机障碍物
%rand_r = randi([1 rows], 100, 1); % 总共是100个障碍物， 100 × 1 列向量， 各值范围为1-rows
%rand_c = randi([1 cols], 100, 1); % 总共是100个障碍物， 100 × 1 列向量， 各值范围为1-cols
%obs_sub = [rand_r rand_c]; % 表示的是每个障碍物的坐标(row, col)

dy_search_area = []; % 存放所有遍历节点，用于动态绘制搜索区域
search_head = 100;
search_end = search_head;

%%初始化地图
%初始点、起始点颜色设置、索引设置
field = ones(sz);
%field(start_sub(1), start_sub(2)) = 4; % 单个点的话可以这样设置，多个点的话需要sub2ind，所以obs是多个需要转换
%field(goal_sub(1), goal_sub(2)) = 5;
start_ind = sub2ind(sz, start_sub(1), start_sub(2));
goal_ind = sub2ind(sz, goal_sub(1), goal_sub(2));
field(start_ind) = 4;
field(goal_ind) = 5;

%障碍物颜色设置、索引设置
obs_r = obs_sub(:, 1);
obs_c = obs_sub(:, 2);
obs_ind = sub2ind(sz, obs_r, obs_c);
field(obs_ind) = 2;

%%***************************************绘制格栅地图***************************************
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.2 0.8 0.6];        % 7-翠绿-动态规划的路径
colormap(cmap);
image(1.5, 1.5, field);
grid on;
hold on;
set(gca, 'gridline', '-', 'gridcolor', 'k', 'linewidth', 0.5, 'GridAlpha', 0.5);
set(gca, 'xtick', 1:cols+1, 'ytick', 1:rows+1);
set(gca, 'XAxisLocation', 'top');
axis image;

%%***************************************Dijkstra实现流程***************************************
% S表，第一列表示格栅的ind编号 第二列表示的是从起点到本节点Node已求得的最小距离
% U表，第一列表示格栅的ind编号 第二列表示的是从起点到本节点Node暂时求得的最小距离
% U表n×2的向量，第一列是位置ind索引，第二列是起点到改点的距离
U(:, 1) = (1:rows*cols)'; % 将格栅图的所有ind坐标存到U的第一列
U(:, 2) = inf; % 初始是起点到各个ind的位置为inf
U(start_ind, :) = []; % 将起始点信息从U表中删除
S = [start_ind 0]; % 将起点放进S表中 起点到起点的距离为0

%更新起点的相邻节点以及代价
next_nodes = Dijkstra_NextNodes(rows, cols, start_ind, field);

%将起点周围的点更新在U中，即在U中的位置
for i = 1:8
    next = next_nodes(i, 1); % 将第一个子节点的ind存放在next中
    %判断节点是否存在，然后将子节点的代价（距离）更新到U第二列中
    if ~isinf(next) % 存在的话isinf(next)会是0因为next是一个确切的值不是inf 加个~的话整体值是1
        index = find(U(:, 1) == next); % find找非0的索引即ind值，
        U(index, 2) = next_nodes(i, 2); % 找到这个节点在U中的位置之后，对应的第二列的dist_cost做相应的修改
    end
end

%path是n×2的向量，第一列存放地图上点的索引位置，第二列存放父节点到子节点的可达路径
for i = 1:rows*cols
    path{i, 1} = i; % 把所有的图上的点都存进去path cell数组
end

%更新起点到周围8个点的路径
% 注:起点到任意有效点的路径都有，但是是否为最短路径，通过U集合比较来判断
% 注:程序在这一步，并没有进行最短路径的比较
for i = 1:8
    next = next_nodes(i, 1); % 将此子节点存放到next
    if ~isinf(next_nodes(i, 2)) % 如果父节点到此子节点的距离不为inf也就是两者之间有距离
        % 此时将起始节点start_ind和起始节点start_ind到next这个子节点的欧氏距离存放到对应的子节点ind的path中第二列cell数组中
        path{next, 2} = [start_ind, next_nodes(i, 1)];
    end
end

%检查上述节点数据不为空之后进行循环遍历
while ~isempty(U) % U非空
    % 此时是第一次从起点开始的8个子节点求最小值（min具体用法自行mathworks）最小值对应此次U中的index(1,2,...8)
    [dist_min, index] = min(U(:, 2));
    node = U(index, 1); % 找到最小值的ind
    S(end+1, :) = [node, dist_min]; % 将这个最短距离的节点存和距离到S表中
    U(index, :) = []; % 将这个节点从U表中去除

    %绘制动态搜索区域即所有node节点,动态区域不包括起点终点和障碍物
    if find(node == obs_ind)
    elseif node == goal_ind
    else
        dy_search_area(end+1) = node; % 动态搜索区域
        field(dy_search_area) = 7;
        if mod(search_head, search_end) == 0 % 控制动图绘制速度
            image(1.5, 1.5, field);
            drawnow; %更新图窗并处理任何挂起的回调。如果您修改图形对象并且需要在屏幕上立即查看这次更新。
        end
    end
    search_head = search_head + 10000; % 更新刷新速度

    %从当前的node节点开始当做其实节点进行遍历，判断子节点是否自U集合中，更新子节点的距离
    next_nodes = Dijkstra_NextNodes(rows, cols, node, field);
    for i = 1:8
        next = next_nodes(i, 1);

        %判断子节点是否为空，并且不能在S集合（已选择的节点）里
        if ~isinf(next) && ~ismember(next, S(:, 1))
            index_U = find(next == U(:, 1));
            cost = next_nodes(i, 2);

            %判断是否更新，此时的next在U中的距离为inf
            if dist_min + cost < U(index_U, 2)
                U(index_U, 2) = dist_min + cost;
                path{next, 2} = [path{node, 2}, next]; % 更新路径，此时next位置上的第二列就是起点到next的最短路径点的ind
            end
        end
    end
end

%找到最优路径, 我只要终点的，也可以选择其他点
optimal_path = path{goal_ind, 2};
best_path = [20 95 114 172 290 309 364 381];
%绘制最优路径折线图
image(1.5, 1.5, field);
hold on;
field(dy_search_area) = 1;
[plotr, plotc] = ind2sub(sz, optimal_path);
[plot_r, plot_c] = ind2sub(sz, best_path);
plot(plotc + 0.5, plotr + 0.5, '-b', 'LineWidth', 2);
plot(plot_c + 0.5, plot_r + 0.5, '-r', 'LineWidth', 2);

scatter(plotc + 0.5, plotr + 0.5, 30, 'filled', MarkerEdgeColor='b', MarkerFaceColor='red');
%set(gca, 'xticklabel',[], 'yticklabel',[]) % 隐藏坐标轴的数字
%set(gca,'xtick',[], 'ytick',[]) % 隐藏坐标轴的刻度 也可以复合写

2.Dijkstra_NextNodes.m文件

function next_nodes = Dijkstra_NextNodes(rows, cols, parent_node_ind, field)
%首先将parent_node_ind的ind坐标转换为sub坐标
sz = [rows cols];
[row, col] = ind2sub(sz, parent_node_ind); % 先转换为行列表示方便后续8个方向上的移动
next_nodes = inf(8, 2); % 初始父节点下一个节点即8个方向上的子节点为inf， 后续存放更新的信息，大小8×2， 第一列表示子节点的位置，第二列表示父子的距离信息
move_pos = [1, 1; 1, 0; 0, 1; -1, 1; -1, 0; -1, -1; 0, -1; 1, -1]; % 8×2

%更新next_nodes，子节点坐标ind和父子距离
for i = 1:8
    %前提是不能超出边界做判断,因为是行列表示,所以行列的坐标肯定要>0
    if 0 < row + move_pos(i, 1) && row + move_pos(i, 1) <= rows && 0 < col + move_pos(i, 2) && col + move_pos(i, 2) <= cols
        temp = [row + move_pos(i, 1), col + move_pos(i, 2)]; % 临时存放当前移动过去的子节点
        ind = sub2ind(sz, temp(1), temp(2)); % 将当前子节点的sub坐标转换为ind坐标
        next_nodes(i, 1) = ind; % 将当前子节点ind存放到next_nodes中的第一列

        %判断当前子节点是否是障碍物不是的话计算父子的欧式距离
        if field(ind) ~= 2 % 不是障碍物 注：如果是障碍物不用管默认为inf
            dist_cost = norm([row col] - temp); % 一定得是行列表示法进行欧氏距离计算
            next_nodes(i, 2) = dist_cost; % 更新父子节点的距离存放到next_nodes中的第二列
        end
    end
end

end

3.obs_sub.mat障碍物

写法很多，随机的也行固定的也行，或者不用加载mat文件直接在Dijkstra_NextNodes.m文件这样写obs_sub=[1, 2; 3, 4]等随便在里面添加前提是在地图范围内。

14    20
3    4
15    6
3    8
3    2
13    14
7    9
14    20
15    9
12    13
15    4
5    8
15    4
20    16
18    18
2    8
8    14
8    6
14    11
12    17
16    12
8    7
5    6
2    10
16    9
5    8
8    12
12    15
5    9
13    9
10    3
4    1
16    6
3    7
6    14
5    20
11    19
2    10
9    5
3    16
3    16
16    15
6    15
13    3
20    14
9    10
14    5
16    2
9    17
14    4
3    4
19    14
4    18
6    11
16    15
10    4
16    20
8    11
6    14
1    1
14    17
9    15
10    3
13    11
2    7
7    11
16    8
14    9
3    4
3    6
2    1
1    19
9    14
14    19
15    4
11    19
3    16
13    12
3    9
3    6
2    16
3    5
4    2
4    16
7    14
7    15
5    13
6    9
18    8
15    17
12    7
4    17
5    16
2    18
19    11
15    13
12    20
7    9
4    2
13    18

4.文件结构

最后运行的话在Dijkstra_main.m文件运行。

5.运行结果

---

 OVER