基于Dijkstra算法的路径规划实践(MATLAB语言)

自己平时学习的一些代码记录——自己用

简单地图，50节点的拓扑图

运行结果：

代码部分：

clear 
close all
clc
tic
%t = clock;
%% 地图信息
% 定义元胞数组，每个元素分别存放 节点编号——与其相邻的节点编号——代价
nodes = cell(0);
for i=1:50
    nodes(i,:) = {i,[],[]};
end
nodes(1,2:3) = {2,3};
nodes(2,2:3) = {[1,3,5],[3,5,5]};
nodes(3,2:3) = {[2,4],[5,6]};
nodes(4,2:3) = {[3,10],[6,5]};
nodes(5,2:3) = {[2,6,13],[5,3,3]};
nodes(6,2:3) = {[5,7,11],[6,2,1]};
nodes(7,2:3) = {[6,8],[2,3]};
nodes(8,2:3) = {[7,9,12],[3,1,1]};
nodes(9,2:3) = {[8,10,17],[1,1,3]};
nodes(10,2:3) = {[4,9,18],[5,1,5]};
nodes(11,2:3) = {[6,12,14],[1,2,1]};
nodes(12,2:3) = {[8,11,15],[1,2,1]};
nodes(13,2:3) = {[5,14,19],[3,2,6]};
nodes(14,2:3) = {[11,13,15,21],[1,2,2,4]};
nodes(15,2:3) = {[12,14,16],[1,2,1]};
nodes(16,2:3) = {[15,17,22],[1,2,4]};
nodes(17,2:3) = {[9,16,18,23],[3,2,1,5]};
nodes(18,2:3) = {[10,17,24],[5,1,6]};
nodes(19,2:3) = {[13,20,25],[6,2,3]};
nodes(20,2:3) = {[19,21,26],[2,2,1]};
nodes(21,2:3) = {[14,20,22,28],[4,2,4,4]};
nodes(22,2:3) = {[16,21,23],[4,4,1]};
nodes(23,2:3) = {[17,22,24],[5,1,2]};
nodes(24,2:3) = {[28,23,35],[6,2,7]};
nodes(25,2:3) = {[19,27,29],[3,3,4]};
nodes(26,2:3) = {[20,27],[1,1]};
nodes(27,2:3) = {[25,26,28],[3,1,2]};
nodes(28,2:3) = {[21,27,31],[2,2,1]};
nodes(29,2:3) = {[25,30,39],[4,1,5]};
nodes(30,2:3) = {[29,32,40],[1,1,3]};
nodes(31,2:3) = {[28,32,34],[1,2,6]};
nodes(32,2:3) = {[30,31,33],[1,2,1]};
nodes(33,2:3) = {[32,42],[1,3]};
nodes(34,2:3) = {[31,35,36],[6,5,4]};
nodes(35,2:3) = {[24,34,38],[7,5,5]};
nodes(36,2:3) = {[34,37,43],[4,1,3]};
nodes(37,2:3) = {[36,38,44],[1,2,1]};
nodes(38,2:3) = {[35,37,45],[5,2,2]};
nodes(39,2:3) = {[29,40,46],[5,2,5]};
nodes(40,2:3) = {[30,39,41],[3,2,1]};
nodes(41,2:3) = {[40,42,46],[1,2,3]};
nodes(42,2:3) = {[33,41,43,49],[3,2,3,2]};
nodes(43,2:3) = {[36,42,44,47],[3,3,2,2]};
nodes(44,2:3) = {[37,43,45,48],[1,1,2,3]};
nodes(45,2:3) = {[38,44,48],[2,1,2]};
nodes(46,2:3) = {[39,41,49],[5,3,2]};
nodes(47,2:3) = {[43,48,50],[2,1,3]};
nodes(48,2:3) = {[44,45,47],[3,2,1]};
nodes(49,2:3) = {[42,46,50],[2,2,1]};
nodes(50,2:3) = {[47,49],[3,1]};

%% 算法初始化
% 集合S和集合U的第一列均表示节点编号
% 集合S的第二列表示: （已找到最短路径的节点加入S）从源点到该节点的最终最短距离
% 集合U的第二列表示: 从源点到该节点的当前最短距离
S = [1,0];
U(:,1) = 2:50;
U(1,2) = 3;
U(2:end,2) = inf;%初始时到另外节点都不可达，默认无穷大

%% 最优路径的初始化
% path_opt是最终完全确定的最优路径集合
% path_tmep是暂时的最优路径
% path_temp的第二列就是最短路径
path_opt = cell(50,2);
path_opt(1,:) = {1,1}; %初始时，只有1号节点，1->1

path_temp = cell(50,2);
path_temp(1,:) = {1,1};
path_temp(2,:) = {2,[1,2]};

%% 算法主体
while ~isempty(U)
    % 在集合U中找到当前最小的距离值及对应的节点，添加到S中并从U中删除
    % dist_min是最小距离，i_index是对应的结点在U中的行索引值,node_min才是节点的编号
    [dist_min,node_index] = min(U(:,2));
    node_min = U(node_index,1);
    S(end+1,:) = [node_min,dist_min];%添加到S
    U(node_index,:) = [];%从U中删除
    
    % 将最小距离的节点加入最短路径集合
    path_opt(node_min,:) = path_temp(node_min,:);
    
    %% 找到当前最小距离节点的所有邻接点
    % length是邻接点的个数
    for i = 1:length(nodes{node_min,2})
        % 需要判断的节点
        node_temp = nodes{node_min,2}(i);
        % 找到集合U中node_temp的索引值
        i_index_temp = find(U(:,1) == node_temp);
        % 判断是否更新
        if ~isempty(i_index_temp)
            new_len = dist_min + nodes{node_min,3}(i);
            old_len = U(i_index_temp,2);
            if  new_len < old_len 
                U(i_index_temp,2) = new_len;
                %更新暂时最优路径
                path_temp{node_temp,1} = node_temp;
                path_temp{node_temp,2} = [path_opt{node_min,2},node_temp];
            end
        end
    end
end

len = S(find(S(:,1)==50),2);
fprintf('路径长度：%d\n',len);
fprintf('最优路径为：\n')
for i = 1:length(path_opt{50,2})-1
    fprintf("%d->",path_opt{50,2}(i));
end
fprintf('50\n');



%% 计时部分
% toc
% time = etime(clock,t);
% 
% fid = fopen('timeConsume_Dijkstra.txt','a+');
% 
% fprintf(fid,'%.20f\n',time);
% fclose(fid);