路径规划与轨迹跟踪 1 —— 全局路径规划_Dijkstra算法

视频链接

学习大纲

算法讲解

算法思想

%% 图定义
% 根据节点的邻近节点表及字母节点-数字节点对应表，构造节点元胞数组
nodes_dist = cell(0);% nodes_dist是7*3的cell
% A-1,B-2,C-3,D-4,E-5,F-6,G-7
% 第1列是周边结点，第2列对应是节点之间距离(A->B,A->F,A->G)
nodes_dist(1,:) = {1, [2, 6, 7], [12, 16, 14]};
nodes_dist(2,:) = {2, [1, 3, 6], [12, 10, 7]};
nodes_dist(3,:) = {3, [2, 4, 5, 6], [10, 3, 5, 6]};
nodes_dist(4,:) = {4, [3, 5], [3, 4]};
nodes_dist(5,:) = {5, [3, 4, 6, 7], [5, 4, 2, 8]};
nodes_dist(6,:) = {6, [1, 2, 3, 5, 7], [16, 7, 6, 2, 9]};
nodes_dist(7,:) = {7, [1, 5, 6], [14, 8, 9]};

算法精讲

• 更新C的邻接节点B、F、E，其中B和F肯定更新，E的话要进行路径长度的比较(E->D还是E->C->D)
• 最后更新E节点
• 更新E的邻接节点F、G，G肯定可以更新，F的话进行路径长度的比较(F->C->D还是F->E->D)
• 最后更新F节点

图1与图2初始化程序

%% 算法初始化
% S/U的第一列表示节点编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更；
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更

S = [4, 0];% 第1列 表示节点编号 第2列表示本节点到源节点D的距离长度(初始时D->D的距离为0)
U(:,1) = [1, 2, 3, 5, 6, 7];% 存放还未搜索的最短节点的集合
U(:,2) = [inf, inf, 3, 4, inf, inf];% 存放初始该节点到源节点D的距离，A->D = inf表示A与D不相邻

% 最优路径及暂时最优路径的初始化
path_opt = cell(7,2);% 最优路径集
% 初始化时，第1个4表示起始节点编号D，第2个4表示最优路线经过节点D
path_opt(4,:) = {4, 4};

% 暂时最优路径的初始化
path_temp = cell(7,2);
% 第1个"4"表示起始节点编号D，第2个"3"表示目前规划的最优路线经过节点D和C
path_temp(3,:) = {3, [4, 3]};
path_temp(4,:) = {4, 4};
path_temp(5,:) = {5, [4, 5]};

% Dijkstra算法
clc
clear
close all

%% 图定义
% 根据节点的邻近节点表及字母节点-数字节点对应表，构造节点元胞数组
nodes_dist = cell(0);% nodes_dist是7*3的cell
% A-1,B-2,C-3,D-4,E-5,F-6,G-7
% 第1列是周边结点，第2列对应是节点之间距离(A->B,A->F,A->G)
nodes_dist(1,:) = {1, [2, 6, 7], [12, 16, 14]};
nodes_dist(2,:) = {2, [1, 3, 6], [12, 10, 7]};
nodes_dist(3,:) = {3, [2, 4, 5, 6], [10, 3, 5, 6]};
nodes_dist(4,:) = {4, [3, 5], [3, 4]};
nodes_dist(5,:) = {5, [3, 4, 6, 7], [5, 4, 2, 8]};
nodes_dist(6,:) = {6, [1, 2, 3, 5, 7], [16, 7, 6, 2, 9]};
nodes_dist(7,:) = {7, [1, 5, 6], [14, 8, 9]};

%% 算法初始化
% S/U的第一列表示节点编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更；
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更

S = [4, 0];% 第1列 表示节点编号 第2列表示本节点到源节点D的距离长度(初始时D->D的距离为0)
% 问题：不知道这里是用列向量还是行向量，原程序是行向量
U(:,1) = [1, 2, 3, 5, 6, 7];% 存放还未搜索的最短节点的集合
U(:,2) = [inf, inf, 3, 4, inf, inf];% 存放初始该节点到源节点D的距离，A->D = inf表示A与D不相邻

% 最优路径及暂时最优路径的初始化
path_opt = cell(7,2);% 最优路径集
% 初始化时，第1个4表示起始节点编号D，第2个4表示最优路线经过节点D
path_opt(4,:) = {4, 4};

% 暂时最优路径的初始化
path_temp = cell(7,2);
% 第1个"4"表示起始节点编号D，第2个"3"表示目前规划的最优路线经过节点D和C
path_temp(3,:) = {3, [4, 3]};
path_temp(4,:) = {4, 4};
path_temp(5,:) = {5, [4, 5]};

%% 循环遍历所有节点
while ~isempty(U)
    
    % 在U集合找出当前最小距离值及对应节点,并移除该节点至S集合中
    [dist_min, idx] = min(U(:,2));% idx表示U集合中最短路径值的索引，dist_min表示最短路径值
    node_min = U(idx, 1);% 在U集合中找到最小节点，比如第1次在U集合中路径最小为3，找到对应的节点C->3
    S(end+1,:) = [node_min, dist_min];% 将该节点添加到集合S中
    U(idx,:) = [];% 将U中添加到S集合的节点清除
    
    % 将最小距离值的节点添加到最优路径集合
    path_opt(node_min,:) = path_temp(node_min,:);
    
    %% 依次遍历最小距离节点的邻节点，判断是否在U集合中更新邻节点的距离值
    for i = 1:length(nodes_dist{node_min, 2})% 例如找到C节点周边的其他节点
        
        % 需要判断的节点
        node_temp = nodes_dist{node_min, 2}(i);% 例如找节点C的第2列是与C相连的其他节点，找到B->2
        
        % 找出U集合中节点node_temp的索引值
        idx_temp = find(node_temp == U(:,1));% 在U结合中找到B->的索引值(这里find的是数字2)
        
        % 判断是否更新
        if ~isempty(idx_temp)
            if dist_min + nodes_dist{node_min, 3}(i) < U(idx_temp, 2)
                U(idx_temp, 2) = dist_min + nodes_dist{node_min, 3}(i);
                
                % 更新暂时最优路径
                path_temp{node_temp, 1} = node_temp;
                path_temp{node_temp, 2} = [path_opt{node_min, 2}, node_temp];                
            end
        end
    end
end